CN113405548A - 一种电驱动哺乳构型足式机器人足地滑移估计方法 - Google Patents

一种电驱动哺乳构型足式机器人足地滑移估计方法 Download PDF

Info

Publication number
CN113405548A
CN113405548A CN202110646523.3A CN202110646523A CN113405548A CN 113405548 A CN113405548 A CN 113405548A CN 202110646523 A CN202110646523 A CN 202110646523A CN 113405548 A CN113405548 A CN 113405548A
Authority
CN
China
Prior art keywords
foot
robot
configuration
slip
type robot
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.)
Pending
Application number
CN202110646523.3A
Other languages
English (en)
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.)
Intelligent Mobile Robot Zhongshan Research Institute
China North Vehicle Research Institute
Original Assignee
Intelligent Mobile Robot Zhongshan Research Institute
China North Vehicle Research Institute
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 Intelligent Mobile Robot Zhongshan Research Institute, China North Vehicle Research Institute filed Critical Intelligent Mobile Robot Zhongshan Research Institute
Priority to CN202110646523.3A priority Critical patent/CN113405548A/zh
Publication of CN113405548A publication Critical patent/CN113405548A/zh
Pending legal-status Critical Current

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
    • 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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及一种电驱动哺乳构型足式机器人足地滑移估计方法，属于足式机器人技术领域。通过对一种电驱动哺乳构型足式机器人足地滑移估计方法的研究，使得本发明所提哺乳构型足式机器人在牵引力方向移动过程中具有滑移状态实时检测功能，并可以通过检测到的足地滑移情况实现足地滑移抑制功能，并提高足式机器人控制的运动稳定性。

Description

一种电驱动哺乳构型足式机器人足地滑移估计方法
技术领域
本发明属于足式机器人技术领域，具体涉及一种电驱动哺乳构型足式机器人足地滑移估计方法。
背景技术
电驱动足式机器人足地滑移估计是足式系统稳定控制的重要关键技术，承担着外部环境扰动转化为具体的运动控制的重要任务。以保证足式平台运动稳定性、灵活性、鲁棒性以及操作便易性为追求目标，哺乳构型足式机器人足地滑移估计方法需要重点关注足式系统行走功能及稳定性控制性能。
目前，我国足式机器人起步较晚，足式机器人普遍存在自主稳定性不高，地形适应性差，外部滑移扰动未知，对工作地形环境的依赖性强等缺点。足式机器人在结构上属于串并联混合结构，在控制方面属于多输入多输出的冗余耦合系统，足式机器人由于自由度繁多从而增加了控制系统的复杂性，在运动过程中缺乏对实时足地滑移估计的能力，目前足式机器人研究主要面向野外崎岖地面，大多注重于步行功能的实现，对足式机器人滑移问题关注较少。
发明内容
(一)要解决的技术问题
本发明要解决的技术问题是：如何解决机器人普遍存在自主稳定性不高，地形适应性差，外部滑移扰动未知，对工作环境的依赖性强，自由度繁多等缺点从而增加了控制系统的复杂性以及运动过程中缺乏对实时足地滑移估计能力的问题，从而提出一种电驱动哺乳构型足式机器人足地滑移估计方法。
(二)技术方案
为了解决上述技术问题，本发明提供了一种电驱动哺乳构型足式机器人足地滑移估计方法，包括以下步骤：
步骤一、设计哺乳构型足式机器人本体传感数据解算方法，得到的机器人在导航坐标系中的加速度；
步骤二、将步骤一验证得到的机器人在导航坐标系中的加速度，作为基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架的输入变量，从而建立基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架，得到哺乳构型足式机器人位置、速度估计变量；
步骤三、将步骤二得到的哺乳构型足式机器人位置、速度估计变量，用于作为单腿动力学方程的输入变量，建立哺乳构型足式机器人足地滑移估计模型，得到哺乳构型足式机器人足地滑移加速度模型；
步骤四、利用步骤三得到的哺乳构型足式机器人足地滑移加速度模型，建立基于扩展卡尔曼滤波的哺乳构型足地滑移状态估计方法；
步骤五、基于步骤一至步骤四进行哺乳构型足式机器人的足地滑移检测。
优选地，步骤一中，对惯导等传感器的数据进行标定及补偿，通过标定加速度及角加速度信号来抑制哺乳构型牵引力运动方向速度和姿态解算的误差，最终输出准确的哺乳构型机器人牵引力方向本体速度及三维姿态信息，惯导传感实时数据具体解算结果为：
Figure BDA0003109972710000021
式中，
Figure BDA0003109972710000022
为导航坐标系到机体坐标系的转换矩阵，fB为空间中的真实加速度和重力导致的加速度之差的力矢量，gn为重力矢量，
Figure BDA0003109972710000023
为地球自转角度在导航坐标系中的投影，
Figure BDA0003109972710000024
为机器人在导航坐标系中的速度，vn为机器人在导航坐标系中的速度。
优选地，步骤二中，将步骤一验证得到的机器人在导航坐标系中的加速度
Figure BDA0003109972710000031
作为基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架的输入变量
Figure BDA0003109972710000032
从而建立基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架。
优选地，步骤二中，根据足式机器人单腿及质心状态变量，建立状态估计过程中的预测模型，其具体建模结果为：
Figure BDA0003109972710000033
式中，f为三维方向的加速度真实值；bf为加速度计的偏差；bw为角加速度计的偏差；wf为加速度计的随机误差；ww为角加速度计的随机误差；w为三维方向的角速率真实值；
Figure BDA0003109972710000034
为三维方向的加速度测量值；
Figure BDA0003109972710000035
为三维方向的角速率测量值；wbf为加速度计噪声矩阵；wbw为角加速度计噪声矩阵。
建立状态估计过程中的测量模型，其具体建模结果为：
Figure BDA0003109972710000036
式中，pc为机器人的位置修正量；vc为机体的速度修正量；p为机器人的位置测量变量；v为机体的速度测量变量。
根据状态变量的预测更新和测量更新，基于反馈校正方法得到足式机器人的状态变量结果如下所示：
Figure BDA0003109972710000041
式中，
Figure BDA0003109972710000042
为位置估计值，
Figure BDA0003109972710000043
为速度估计值，Δt为时间周期变量；
Figure BDA0003109972710000044
为导航坐标系转换矩阵；
优选地，步骤三中，将步骤二公式(4)计算得到的哺乳构型足式机器人位置、速度估计变量，用于作为单腿动力学方程的输入变量，建立哺乳构型足式机器人足地滑移估计模型.
优选地，步骤三中，首先建立足式机器人单腿滑移的动力学模型，根据哺乳构型足式机器人行走时的单腿滑移模型，得到两连杆的位置矢量：
Figure BDA0003109972710000045
式中，LPsy为牵引方向滑移距离，LPsz为法向方向滑移距离，MH为大腿连杆的质量，LH为大腿长度；MK为小腿连杆的质量，LK为小腿长度，qH为髋关节角度，qK为膝关节角度；
定义滑移状态下的动力学系统输入变量为q＝[qK qH LPsy LPsz]T，足-地接触力定义为F＝[FT FN]T，FT代表足端所受到的切向力，FN代表足端所受到的法向力。
基于拉格朗日方程，建立单腿三自由度的运动动力学方程，如下所示：
Figure BDA0003109972710000046
式中，Dd(q)为惯性矩阵，
Figure BDA0003109972710000047
为哥氏力矩阵，Gd(q)为重力矩阵，Cd为重力矩阵，q为关节角度，
Figure BDA0003109972710000051
为关节角速度，
Figure BDA0003109972710000052
为关节角加速度，u为重力矩阵，Ed为重力矩阵，F为足力矢量。
根据单腿滑移动力学运动方程(6)，建立哺乳构型足端牵引力方向滑移加速度模型，如下所示：
Figure BDA0003109972710000053
式中FT为足地牵引向力参数，MH为大腿连杆的质量，LH为大腿长度，MK为小腿连杆的质量，LK为小腿长度，qH为髋关节角度，qK为膝关节角度。
优选地，步骤四中，利用步骤三计算得到的哺乳构型足式机器人足地滑移加速度模型公式(7)，建立基于扩展卡尔曼滤波的哺乳构型足地滑移状态估计方法.
优选地，步骤四中，建立哺乳构型足式机器人股节和胫节的转动速度变量分别wHz，wAz：
Figure BDA0003109972710000054
Figure BDA0003109972710000055
式中wHx,wHy,wHz分别为股节沿牵引、侧向和法向的转动速度变量；wKx,wKy,wKz分别为胫节沿牵引、侧向和法向的转动速度变量。
根据股节和胫节的转动速度，建立哺乳构型足式机器人足地滑移状态变量方程：
Figure BDA0003109972710000061
式中
Figure BDA0003109972710000062
为足地滑移速度变量，fs(ys,ws1)为足地滑移加速度变量。
最终根据公式(7)得到的哺乳构型足式机器人的足端滑移加速度为：
Figure BDA0003109972710000063
式中FT为足地牵引向力参数，MH为大腿连杆的质量，LH为大腿长度，MK为小腿连杆的质量，LK为小腿长度，qH为髋关节角度，qK为膝关节角度；
哺乳构型足式机器人的足端滑移速度解算为：
Figure BDA0003109972710000064
式中，
Figure BDA0003109972710000065
为足地速度矢量，
Figure BDA0003109972710000066
为状态估计得到的基节的侧向速度。
哺乳构型足式机器人的足端滑移位置解算为：
Figure BDA0003109972710000067
式中，LPsy为足地位置矢量，qK0为膝关节初始角度，qH0为髋关节初始角度，
Figure BDA0003109972710000068
为状态估计得到的基节的侧向速度，Δt为时间周期变量。
根据式(11)，(12)，(13)最终得到哺乳构型足式机器人的滑移状态方程为：
Figure BDA0003109972710000069
式中wsy为白噪声矩阵，协方差矩阵为Qsy。
基于扩展卡尔曼滤波的循环迭代方法对两种测量数据的信息进行融合得到足地滑移估计值ps(ys,ws3)，如图4所示，定义哺乳构型足式机器人牵引方向的足地滑移率SI(t)为：
Figure BDA0003109972710000071
LPyei为控制周期结束时，由正运动学计算出的足端在机器人坐标系下的位置；LPysi为控制周期开始时，由正运动学计算出的足端在机器人坐标系下的位置；当滑移率处于SI(t)∈[0,1)这个范围内时，证明哺乳构型足式机器人的运动过程为有效运动。
优选地，步骤五中，基于步骤一至步骤四得到的哺乳构型足式机器人的足地滑移检测流程如下：
步骤1：哺乳构型足式机器人的足地滑移检测起始于足端落地时刻，通过足力传感器检测落地时刻；
步骤2：利用步骤1足力传感器检测到的足地落地时刻状态，进行支撑相和摆动相的切换，通过哺乳构型足式机器人本体传感数据解算得到惯导和运动学状态变量；
步骤3：利用步骤2计算得到的惯导和运动学状态变量，作为状态估计框架中的输入变量。通过基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架，基于惯导、运动学及足力数据融合得到的哺乳构型足式机器人位置、速度状态变量；
步骤4：利用步骤3计算得到的位置、速度、姿态状态变量，作为滑移估计的输入变量。通过哺乳构型机器人足地滑移估计模型，建立哺乳构型足端牵引力方向滑移加速度模型；
步骤5：利用步骤4计算得到的滑移加速度模型，作为足地滑移状态估计的判定基础。通过基于扩展卡尔曼滤波的哺乳构型足地滑移状态估计方法，建立哺乳构型足式机器人的足地滑移率检测条件，检测足地滑移情况；
步骤6：利用步骤5判定得到的足地滑移情况，通过足力传感器检测判断支撑相是否结束，当支撑相结束后通过步态切换转变成摆动相。
本发明还提供了一种所述方法在足式机器人技术领域中的应用。
(三)有益效果
通过对一种电驱动哺乳构型足式机器人足地滑移估计方法的研究，使得本发明所提哺乳构型足式机器人在牵引力方向移动过程中具有滑移状态实时检测功能，并可以通过检测到的足地滑移情况实现足地滑移抑制功能，并提高足式机器人控制的运动稳定性。
附图说明
图1是本发明的一种电驱动哺乳构型足式机器人足地滑移估计方法流程图；
图2是本发明的哺乳构型足式机器人的牵引力方向机器人本体运动示意图；
图3是本发明的哺乳构型足式机器人的单腿运动示意图；
图4是本发明的哺乳构型足式机器人的足地滑移状态估计框图；
图5是本发明的哺乳构型足式机器人的足地滑移检测流程图。
具体实施方式
为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。
如图1所示，本发明提供的一种电驱动哺乳构型足式机器人足地滑移估计方法具体是按照以下步骤完成的：
步骤一、设计哺乳构型足式机器人本体传感数据解算方法：对惯导等传感器的数据进行标定及补偿，通过标定加速度及角加速度信号来抑制哺乳构型牵引力运动方向速度和姿态解算的误差，最终输出准确的哺乳构型机器人牵引力方向本体速度及三维姿态信息，惯导传感实时数据具体解算结果为：
Figure BDA0003109972710000091
式中，
Figure BDA0003109972710000092
为导航坐标系到机体坐标系的转换矩阵，fB为空间中的真实加速度和重力导致的加速度之差的力矢量，gn为重力矢量，
Figure BDA0003109972710000093
为地球自转角度在导航坐标系中的投影，
Figure BDA0003109972710000094
为机器人在导航坐标系中的速度，vn为机器人在导航坐标系中的速度；
步骤二、将步骤一验证得到的机器人在导航坐标系中的加速度
Figure BDA0003109972710000095
作为基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架的输入变量
Figure BDA0003109972710000096
从而建立基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架；
具体为：
根据足式机器人单腿及质心状态变量，建立状态估计过程中的预测模型，其具体建模结果为：
Figure BDA0003109972710000101
式中，f为三维方向的加速度真实值；bf为加速度计的偏差；bw为角加速度计的偏差；wf为加速度计的随机误差；ww为角加速度计的随机误差；w为三维方向的角速率真实值；
Figure BDA0003109972710000102
为三维方向的加速度测量值；
Figure BDA0003109972710000103
为三维方向的角速率测量值；wbf为加速度计噪声矩阵；wbw为角加速度计噪声矩阵。
建立状态估计过程中的测量模型，其具体建模结果为：
Figure BDA0003109972710000104
式中，pc为机器人的位置修正量；vc为机体的速度修正量；p为机器人的位置测量变量；v为机体的速度测量变量。
根据状态变量的预测更新和测量更新，基于反馈校正方法得到足式机器人的状态变量结果如下所示：
Figure BDA0003109972710000105
式中，
Figure BDA0003109972710000106
为位置估计值，
Figure BDA0003109972710000107
为速度估计值，Δt为时间周期变量；
Figure BDA0003109972710000108
为导航坐标系转换矩阵；
步骤三、将步骤二公式(4)计算得到的哺乳构型足式机器人位置、速度估计变量，用于作为单腿动力学方程的输入变量，建立哺乳构型足式机器人足地滑移估计模型；
具体地，首先建立足式机器人单腿滑移的动力学模型，根据哺乳构型足式机器人行走时的单腿滑移模型，哺乳构型足式机器人机体运动方式如图2所示，哺乳构型机器人单腿滑移运动方式如图3所示，由下式得到两连杆的位置矢量：
Figure BDA0003109972710000111
式中，LPsy为牵引方向滑移距离，LPsz为法向方向滑移距离，MH为大腿连杆的质量，LH为大腿长度；MK为小腿连杆的质量，LK为小腿长度，qH为髋关节角度，qK为膝关节角度；
定义滑移状态下的动力学系统输入变量为q＝[qK qH LPsy LPsz]T，足-地接触力定义为F＝[FT FN]T，FT代表足端所受到的切向力，FN代表足端所受到的法向力。
基于拉格朗日方程，建立单腿三自由度的运动动力学方程，如下所示：
Figure BDA0003109972710000112
式中，Dd(q)为惯性矩阵，
Figure BDA0003109972710000113
为哥氏力矩阵，Gd(q)为重力矩阵，Cd为重力矩阵，q为关节角度，
Figure BDA0003109972710000114
为关节角速度，
Figure BDA0003109972710000115
为关节角加速度，u为重力矩阵，Ed为重力矩阵，F为足力矢量。
根据单腿滑移动力学运动方程(6)，建立哺乳构型足端牵引力方向滑移加速度模型，如下所示：
Figure BDA0003109972710000121
式中FT为足地牵引向力参数，MH为大腿连杆的质量，LH为大腿长度，MK为小腿连杆的质量，LK为小腿长度，qH为髋关节角度，qK为膝关节角度。
步骤四、利用步骤三计算得到的哺乳构型足式机器人足地滑移加速度模型公式(7)，建立基于扩展卡尔曼滤波的哺乳构型足地滑移状态估计方法；
具体地，建立哺乳构型足式机器人股节和胫节的转动速度变量分别wHz，wAz：
Figure BDA0003109972710000122
Figure BDA0003109972710000123
式中wHx,wHy,wHz分别为股节沿牵引、侧向和法向的转动速度变量；wKx,wKy,wKz分别为胫节沿牵引、侧向和法向的转动速度变量。
根据股节和胫节的转动速度，建立哺乳构型足式机器人足地滑移状态变量方程：
Figure BDA0003109972710000124
式中
Figure BDA0003109972710000125
为足地滑移速度变量，fs(ys,ws1)为足地滑移加速度变量。
最终根据公式(7)得到的哺乳构型足式机器人的足端滑移加速度为：
Figure BDA0003109972710000131
式中FT为足地牵引向力参数，MH为大腿连杆的质量，LH为大腿长度，MK为小腿连杆的质量，LK为小腿长度，qH为髋关节角度，qK为膝关节角度。
哺乳构型足式机器人的足端滑移速度解算为：
Figure BDA0003109972710000132
式中，
Figure BDA0003109972710000133
为足地速度矢量，
Figure BDA0003109972710000134
为状态估计得到的基节的侧向速度。
哺乳构型足式机器人的足端滑移位置解算为：
Figure BDA0003109972710000135
式中，LPsy为足地位置矢量，qK0为膝关节初始角度，qH0为髋关节初始角度，
Figure BDA0003109972710000136
为状态估计得到的基节的侧向速度，Δt为时间周期变量。
根据式(11)，(12)，(13)最终得到哺乳构型足式机器人的滑移状态方程为：
Figure BDA0003109972710000137
式中wsy为白噪声矩阵，协方差矩阵为Qsy。
基于扩展卡尔曼滤波的循环迭代方法对两种测量数据的信息进行融合得到足地滑移估计值ps(ys,ws3)，如图4所示，定义哺乳构型足式机器人牵引方向的足地滑移率SI(t)为：
Figure BDA0003109972710000138
LPyei为控制周期结束时，由正运动学计算出的足端在机器人坐标系下的位置；LPysi为控制周期开始时，由正运动学计算出的足端在机器人坐标系下的位置；当滑移率处于SI(t)∈[0,1)这个范围内时，证明哺乳构型足式机器人的运动过程为有效运动。
步骤五、基于步骤一至步骤四进行哺乳构型足式机器人的足地滑移检测，分为以下6个步骤进行，如图5所示：
步骤1：哺乳构型足式机器人的足地滑移检测起始于足端落地时刻，通过足力传感器检测落地时刻；
步骤2：利用步骤1足力传感器检测到的足地落地时刻状态，进行支撑相和摆动相的切换，通过哺乳构型足式机器人本体传感数据解算得到惯导和运动学状态变量；
步骤3：利用步骤2计算得到的惯导和运动学状态变量，作为状态估计框架中的输入变量。通过基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架，基于惯导、运动学及足力数据融合得到的哺乳构型足式机器人位置、速度、姿态状态变量；
步骤4：利用步骤3计算得到的位置、速度状态变量，作为滑移估计的输入变量。通过哺乳构型机器人足地滑移估计模型，建立哺乳构型足端牵引力方向滑移加速度模型；
步骤5：利用步骤4计算得到的滑移加速度模型，作为足地滑移状态估计的判定基础。通过基于扩展卡尔曼滤波的哺乳构型足地滑移状态估计方法，建立哺乳构型足式机器人的足地滑移率检测条件，检测足地滑移情况；
步骤6：利用步骤5判定得到的足地滑移情况，通过足力传感器检测判断支撑相是否结束，当支撑相结束后通过步态切换转变成摆动相。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (10)

1.一种电驱动哺乳构型足式机器人足地滑移估计方法，其特征在于，包括以下步骤：
步骤一、设计哺乳构型足式机器人本体传感数据解算方法，得到的机器人在导航坐标系中的加速度；
步骤二、将步骤一验证得到的机器人在导航坐标系中的加速度，作为基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架的输入变量，从而建立基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架，得到哺乳构型足式机器人位置、速度估计变量；
步骤三、将步骤二得到的哺乳构型足式机器人位置、速度估计变量，用于作为单腿动力学方程的输入变量，建立哺乳构型足式机器人足地滑移估计模型，得到哺乳构型足式机器人足地滑移加速度模型；
步骤四、利用步骤三得到的哺乳构型足式机器人足地滑移加速度模型，建立基于扩展卡尔曼滤波的哺乳构型足地滑移状态估计方法；
步骤五、基于步骤一至步骤四进行哺乳构型足式机器人的足地滑移检测。
2.如权利要求1所述的方法，其特征在于，步骤一中，对惯导等传感器的数据进行标定及补偿，通过标定加速度及角加速度信号来抑制哺乳构型牵引力运动方向速度和姿态解算的误差，最终输出准确的哺乳构型机器人牵引力方向本体速度及三维姿态信息，惯导传感实时数据具体解算结果为：
Figure FDA0003109972700000011
式中，
Figure FDA0003109972700000012
为导航坐标系到机体坐标系的转换矩阵，fB为空间中的真实加速度和重力导致的加速度之差的力矢量，gn为重力矢量，
Figure FDA0003109972700000021
为地球自转角度在导航坐标系中的投影，
Figure FDA0003109972700000022
为机器人在导航坐标系中的速度，vn为机器人在导航坐标系中的速度。
3.如权利要求2所述的方法，其特征在于，步骤二中，将步骤一验证得到的机器人在导航坐标系中的加速度
Figure FDA0003109972700000023
作为基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架的输入变量
Figure FDA0003109972700000024
从而建立基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架。
4.如权利要求3所述的方法，其特征在于，步骤二中，根据足式机器人单腿及质心状态变量，建立状态估计过程中的预测模型，其具体建模结果为：
Figure FDA0003109972700000025
式中，f为三维方向的加速度真实值；bf为加速度计的偏差；bw为角加速度计的偏差；wf为加速度计的随机误差；ww为角加速度计的随机误差；w为三维方向的角速率真实值；
Figure FDA0003109972700000026
为三维方向的加速度测量值；
Figure FDA0003109972700000027
为三维方向的角速率测量值；wbf为加速度计噪声矩阵；wbw为角加速度计噪声矩阵。
建立状态估计过程中的测量模型，其具体建模结果为：
Figure FDA0003109972700000028
式中，pc为机器人的位置修正量；vc为机体的速度修正量；p为机器人的位置测量变量；v为机体的速度测量变量。
根据状态变量的预测更新和测量更新，基于反馈校正方法得到足式机器人的状态变量结果如下所示：
Figure FDA0003109972700000031
式中，
Figure FDA0003109972700000032
为位置估计值，
Figure FDA0003109972700000033
为速度估计值，Δt为时间周期变量；
Figure FDA0003109972700000034
为导航坐标系转换矩阵。
5.如权利要求4所述的方法，其特征在于，步骤三中，将步骤二公式(4)计算得到的哺乳构型足式机器人位置、速度估计变量，用于作为单腿动力学方程的输入变量，建立哺乳构型足式机器人足地滑移估计模型。
6.如权利要求5所述的方法，其特征在于，步骤三中，首先建立足式机器人单腿滑移的动力学模型，根据哺乳构型足式机器人行走时的单腿滑移模型，得到两连杆的位置矢量：
Figure FDA0003109972700000035
式中，LPsy为牵引方向滑移距离，LPsz为法向方向滑移距离，MH为大腿连杆的质量，LH为大腿长度；MK为小腿连杆的质量，LK为小腿长度，qH为髋关节角度，qK为膝关节角度；
定义滑移状态下的动力学系统输入变量为q＝[qK qH LPsy LPsz]T，足-地接触力定义为F＝[FT FN]T，FT代表足端所受到的切向力，FN代表足端所受到的法向力。
基于拉格朗日方程，建立单腿三自由度的运动动力学方程，如下所示：
Figure FDA0003109972700000041
式中，Dd(q)为惯性矩阵，
Figure FDA0003109972700000042
为哥氏力矩阵，Gd(q)为重力矩阵，Cd为重力矩阵，q为关节角度，
Figure FDA0003109972700000043
为关节角速度，
Figure FDA0003109972700000044
为关节角加速度，u为重力矩阵，Ed为重力矩阵，F为足力矢量。
根据单腿滑移动力学运动方程(6)，建立哺乳构型足端牵引力方向滑移加速度模型，如下所示：
Figure FDA0003109972700000045
式中FT为足地牵引向力参数，MH为大腿连杆的质量，LH为大腿长度，MK为小腿连杆的质量，LK为小腿长度，qH为髋关节角度，qK为膝关节角度。
7.如权利要求6所述的方法，其特征在于，步骤四中，利用步骤三计算得到的哺乳构型足式机器人足地滑移加速度模型公式(7)，建立基于扩展卡尔曼滤波的哺乳构型足地滑移状态估计方法。
8.如权利要求7所述的方法，其特征在于，步骤四中，建立哺乳构型足式机器人股节和胫节的转动速度变量分别wHz，wAz：
Figure FDA0003109972700000046
Figure FDA0003109972700000047
式中wHx,wHy,wHz分别为股节沿牵引、侧向和法向的转动速度变量；wKx,wKy,wKz分别为胫节沿牵引、侧向和法向的转动速度变量。
根据股节和胫节的转动速度，建立哺乳构型足式机器人足地滑移状态变量方程：
Figure FDA0003109972700000051
式中
Figure FDA0003109972700000052
为足地滑移速度变量，fs(ys,ws1)为足地滑移加速度变量。
最终根据公式(7)得到的哺乳构型足式机器人的足端滑移加速度为：
Figure FDA0003109972700000053
式中FT为足地牵引向力参数，MH为大腿连杆的质量，LH为大腿长度，MK为小腿连杆的质量，LK为小腿长度，qH为髋关节角度，qK为膝关节角度；
哺乳构型足式机器人的足端滑移速度解算为：
Figure FDA0003109972700000054
式中，
Figure FDA0003109972700000055
为足地速度矢量，
Figure FDA0003109972700000056
为状态估计得到的基节的侧向速度。
哺乳构型足式机器人的足端滑移位置解算为：
Figure FDA0003109972700000057
式中，LPsy为足地位置矢量，qK0为膝关节初始角度，qH0为髋关节初始角度，
Figure FDA0003109972700000058
为状态估计得到的基节的侧向速度，Δt为时间周期变量。
根据式(11)，(12)，(13)最终得到哺乳构型足式机器人的滑移状态方程为：
Figure FDA0003109972700000061
式中wsy为白噪声矩阵，协方差矩阵为Qsy。
基于扩展卡尔曼滤波的循环迭代方法对两种测量数据的信息进行融合得到足地滑移估计值ps(ys,ws3)，如图4所示，定义哺乳构型足式机器人牵引方向的足地滑移率SI(t)为：
Figure FDA0003109972700000062
LPyei为控制周期结束时，由正运动学计算出的足端在机器人坐标系下的位置；LPysi为控制周期开始时，由正运动学计算出的足端在机器人坐标系下的位置；当滑移率处于SI(t)∈[0,1)这个范围内时，证明哺乳构型足式机器人的运动过程为有效运动。
9.如权利要求8所述的方法，其特征在于，步骤五中，基于步骤一至步骤四得到的哺乳构型足式机器人的足地滑移检测流程如下：
步骤1：哺乳构型足式机器人的足地滑移检测起始于足端落地时刻，通过足力传感器检测落地时刻；
步骤2：利用步骤1足力传感器检测到的足地落地时刻状态，进行支撑相和摆动相的切换，通过哺乳构型足式机器人本体传感数据解算得到惯导和运动学状态变量；
步骤3：利用步骤2计算得到的惯导和运动学状态变量，作为状态估计框架中的输入变量。通过基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架，基于惯导、运动学及足力数据融合得到的哺乳构型足式机器人位置、速度状态变量；
步骤4：利用步骤3计算得到的位置、速度状态变量，作为滑移估计的输入变量，通过哺乳构型机器人足地滑移估计模型，建立哺乳构型足端牵引力方向滑移加速度模型；
步骤5：利用步骤4计算得到的滑移加速度模型，作为足地滑移状态估计的判定基础，通过基于扩展卡尔曼滤波的哺乳构型足地滑移状态估计方法，建立哺乳构型足式机器人的足地滑移率检测条件，检测足地滑移情况；
步骤6：利用步骤5判定得到的足地滑移情况，通过足力传感器检测判断支撑相是否结束，当支撑相结束后通过步态切换转变成摆动相。
10.一种如权利要求1至9中任一项所述方法在足式机器人技术领域中的应用。
CN202110646523.3A 2021-06-10 2021-06-10 一种电驱动哺乳构型足式机器人足地滑移估计方法 Pending CN113405548A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110646523.3A CN113405548A (zh) 2021-06-10 2021-06-10 一种电驱动哺乳构型足式机器人足地滑移估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110646523.3A CN113405548A (zh) 2021-06-10 2021-06-10 一种电驱动哺乳构型足式机器人足地滑移估计方法

Publications (1)

Publication Number Publication Date
CN113405548A true CN113405548A (zh) 2021-09-17

Family

ID=77683392

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110646523.3A Pending CN113405548A (zh) 2021-06-10 2021-06-10 一种电驱动哺乳构型足式机器人足地滑移估计方法

Country Status (1)

Country Link
CN (1) CN113405548A (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104192221A (zh) * 2014-09-26 2014-12-10 哈尔滨工业大学 一种电驱动六足机器人运动控制系统及方法
CN108621161A (zh) * 2018-05-08 2018-10-09 中国人民解放军国防科技大学 基于多传感器信息融合的足式机器人本体状态估计方法
CN111086001A (zh) * 2019-12-25 2020-05-01 广东省智能制造研究所 一种足式机器人多模态感知的状态估计方法及系统
WO2020253854A1 (zh) * 2019-06-21 2020-12-24 台州知通科技有限公司 移动机器人姿态角解算方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104192221A (zh) * 2014-09-26 2014-12-10 哈尔滨工业大学 一种电驱动六足机器人运动控制系统及方法
CN108621161A (zh) * 2018-05-08 2018-10-09 中国人民解放军国防科技大学 基于多传感器信息融合的足式机器人本体状态估计方法
WO2020253854A1 (zh) * 2019-06-21 2020-12-24 台州知通科技有限公司 移动机器人姿态角解算方法
CN111086001A (zh) * 2019-12-25 2020-05-01 广东省智能制造研究所 一种足式机器人多模态感知的状态估计方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
刘宇飞: "面向非预知地形的六足机器人足力优化及滑移抑制研究", 《哈尔滨工业大学博士学位论文》 *

Similar Documents

Publication Publication Date Title
CN110146077A (zh) 移动机器人姿态角解算方法
US8873831B2 (en) Walking robot and simultaneous localization and mapping method thereof
CN108061855B (zh) 一种基于mems传感器的球形电机转子位置检测方法
CN102121828B (zh) 一种仿人机器人身体姿态角度实时估计方法
CN106547206B (zh) 一种基于脚力估计算法的多足机器人动态稳定性定量判定方法
CN107607113B (zh) 一种两轴姿态倾角测量方法
Roan et al. Manipulator state estimation with low cost accelerometers and gyroscopes
Zhang et al. Rider trunk and bicycle pose estimation with fusion of force/inertial sensors
Käslin et al. Towards a passive adaptive planar foot with ground orientation and contact force sensing for legged robots
CN110450159A (zh) 一种双足机器人的足部状态检测装置以及基于该装置的斜面行走方法
Fink et al. Proprioceptive sensor fusion for quadruped robot state estimation
CN113793360A (zh) 基于惯性传感技术的三维人体重构方法
CN113126659B (zh) 一种仿人机器人跳跃着地状态检测系统与方法
CN109556606A (zh) 基于模糊补偿和卡尔曼滤波的传感器角度测量优化方法
Masuya et al. Com motion estimation of a humanoid robot based on a fusion of dynamics and kinematics information
US9802315B2 (en) Controller for mobile robot
CN113358121A (zh) 一种电驱动昆虫构型足式机器人足地滑移估计方法
CN103884351B (zh) 足式机器人运动参数测量方法及其应用
CN113405548A (zh) 一种电驱动哺乳构型足式机器人足地滑移估计方法
Bai et al. Graph-optimisation-based self-calibration method for IMU/odometer using preintegration theory
JP2011255500A (ja) 脚式移動ロボットの制御装置
Yang et al. Novel walking assist device based on mobile manipulator and inertial measurement unit
Ravichandran et al. Joint angle measurement using mems based inertial sensors for biped robot
TW201416815A (zh) 用於六軸運動姿態感測之方法及其感測系統
KR101048716B1 (ko) 2족 로봇 및 2족 로봇의 보행 제어방법

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20210917

RJ01 Rejection of invention patent application after publication