CN113358121A - 一种电驱动昆虫构型足式机器人足地滑移估计方法 - Google Patents

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

Info

Publication number
CN113358121A
CN113358121A CN202110647027.XA CN202110647027A CN113358121A CN 113358121 A CN113358121 A CN 113358121A CN 202110647027 A CN202110647027 A CN 202110647027A CN 113358121 A CN113358121 A CN 113358121A
Authority
CN
China
Prior art keywords
foot
insect
configuration
robot
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
CN202110647027.XA
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 CN202110647027.XA priority Critical patent/CN113358121A/zh
Publication of CN113358121A publication Critical patent/CN113358121A/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/20—Instruments for performing navigational calculations
    • 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
    • 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
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation

Landscapes

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

Abstract

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

Description

一种电驱动昆虫构型足式机器人足地滑移估计方法
技术领域
本发明属于足式机器人技术领域，具体涉及一种电驱动昆虫构型足式机器人足地滑移估计方法。
背景技术
电驱动足式机器人足地滑移估计是足式系统稳定控制的重要关键技术，承担着外部环境扰动转化为具体的运动控制的重要任务。以保证足式平台运动稳定性、灵活性、鲁棒性以及操作便易性为追求目标，昆虫构型足式机器人足地滑移估计方法需要重点关注足式系统行走功能及稳定性控制性能。
目前，我国足式机器人起步较晚，足式机器人普遍存在自主稳定性不高，地形适应性差，外部滑移扰动未知，对工作地形环境的依赖性强等缺点。足式机器人在结构上属于串并联混合结构，在控制方面属于多输入多输出的冗余耦合系统，足式机器人由于自由度繁多从而增加了控制系统的复杂性，在运动过程中缺乏对实时足地滑移估计的能力，目前足式机器人研究主要面向野外崎岖地面，大多注重于步行功能的实现，对足式机器人滑移问题关注较少。
发明内容
(一)要解决的技术问题
本发明要解决的技术问题是：如何解决机器人普遍存在自主稳定性不高，地形适应性差，外部滑移扰动未知，对工作环境的依赖性强，自由度繁多等缺点从而增加了控制系统的复杂性以及运动过程中缺乏对实时足地滑移估计能力的问，而提出一种电驱动昆虫构型足式机器人足地滑移估计方法。
(二)技术方案
为了解决上述技术问题，本发明提供了一种电驱动昆虫构型足式机器人足地滑移估计方法，包括以下步骤：
步骤一、设计昆虫构型足式机器人本体传感数据解算方法，得到机器人在导航坐标系中的加速度；
步骤二、利用步骤一验证得到的机器人在导航坐标系中的加速度，作为状态估计框架中的输入变量，从而建立基于扩展卡尔曼滤波的昆虫构型足式机器人状态估计框架，得到昆虫构型足式机器人位置、速度估计变量；
步骤三、利用步骤二计算得到的昆虫构型足式机器人位置、速度估计变量，用于作为单腿动力学方程的输入变量，建立昆虫构型足式机器人足地滑移估计模型，得到的昆虫构型足式机器人足地滑移加速度模型；
步骤四、利用步骤三计算得到的昆虫构型足式机器人足地滑移加速度模型，建立基于扩展卡尔曼滤波的昆虫构型足地滑移状态估计方法；
步骤五、基于步骤一至步骤四，进行昆虫构型足式机器人的足地滑移检测。
优选地，步骤一中，通过对惯导等传感器的数据进行标定及补偿，通过标定加速度及角加速度信号来抑制昆虫构型牵引力运动方向速度和姿态解算的误差，最终输出准确的昆虫构型机器人牵引力方向本体速度及三维姿态信息，惯导传感实时数据具体解算结果为：
Figure BDA0003110308900000021
式中，
Figure BDA0003110308900000022
为导航坐标系到机体坐标系的转换矩阵，fB为空间中的真实加速度和重力导致的加速度之差的力矢量，gn为重力矢量，
Figure BDA0003110308900000031
为地球自转角度在导航坐标系中的投影，
Figure BDA0003110308900000032
为机器人在导航坐标系中的速度，vn为机器人在导航坐标系中的速度。
优选地，步骤二中，利用步骤一验证得到的机器人在导航坐标系中的加速度
Figure BDA0003110308900000033
作为状态估计框架中的输入变量
Figure BDA0003110308900000034
从而建立基于扩展卡尔曼滤波的昆虫构型足式机器人状态估计框架，得到昆虫构型足式机器人位置、速度估计变量。
优选地，步骤二中，根据足式机器人单腿及质心状态变量，建立状态估计过程中的预测模型，其具体建模结果为：
Figure BDA0003110308900000035
式中，f为三维方向的加速度真实值；bf为加速度计的偏差；bw为角加速度计的偏差；wf为加速度计的随机误差；ww为角加速度计的随机误差；w为三维方向的角速率真实值；
Figure BDA0003110308900000036
为三维方向的加速度测量值；
Figure BDA0003110308900000037
为三维方向的角速率测量值；wbf为加速度计噪声矩阵；wbw为角加速度计噪声矩阵。
建立状态估计过程中的测量模型，其具体建模结果为：
Figure BDA0003110308900000038
式中，pc为机器人的位置修正量；vc为机体的速度修正量；p为机器人的位置测量变量；v为机体的速度测量变量。
根据状态变量的预测更新和测量更新，基于反馈校正方法得到足式机器人的状态变量结果如下所示：
Figure BDA0003110308900000041
式中，
Figure BDA0003110308900000042
为位置估计值，
Figure BDA0003110308900000043
为速度估计值，Δt为时间周期变量；
Figure BDA0003110308900000044
为三维方向的加速度测量值；wf为加速度的偏差值，
Figure BDA0003110308900000045
为导航坐标系转换矩阵。
5、如权利要求4所述的方法，其特征在于，步骤三中，利用步骤二公式(4)计算得到的昆虫构型足式机器人位置、速度估计变量，用于作为单腿动力学方程的输入变量，建立昆虫构型足式机器人足地滑移估计模型，得到的昆虫构型足式机器人足地滑移加速度模型。
优选地，步骤三中，建立足式机器人单腿滑移的动力学模型，根据昆虫构型足式机器人行走时的单腿滑移模型，得到两连杆的位置矢量：
Figure BDA0003110308900000046
式中，LPsx为牵引方向滑移距离，LPsz为法向方向滑移距离，MA为基节连杆的质量，LA为基节长度；MH为大腿连杆的质量，LH为大腿长度；MK为小腿连杆的质量，LK为小腿长度，qA为髋关节角度，qH为髋关节角度，qK为膝关节角度；
定义滑移状态下的动力学系统输入变量为q＝[qA qK qH LPsx LPsz]T，足-地接触力定义为F＝[FT FN]T，FT代表足端所受到的切向力，FN代表足端所受到的法向力；
基于拉格朗日方程，建立单腿三自由度的运动动力学方程，如下所示：
Figure BDA0003110308900000051
式中，Dd(q)为惯性矩阵，
Figure BDA0003110308900000052
为哥氏力矩阵，Gd(q)为重力矩阵，Cd为重力矩阵，q为关节角度，
Figure BDA0003110308900000053
为关节角速度，
Figure BDA0003110308900000054
为关节角加速度，u为重力矩阵，Ed为重力矩阵，F为足力矢量；
根据单腿滑移动力学运动方程(6)，建立昆虫构型足端牵引力方向滑移加速度模型，如下所示：
Figure BDA0003110308900000055
式中FT为足地牵引向力参数，MA为基节连杆的质量，LA为基节长度；MH为大腿连杆的质量，LH为大腿长度；MK为小腿连杆的质量，LK为小腿长度，qA为髋关节角度，qH为髋关节角度，qK为膝关节角度。
优选地，步骤四中，利用步骤三计算得到的昆虫构型足式机器人足地滑移加速度模型(7)，建立基于扩展卡尔曼滤波的昆虫构型足地滑移状态估计方法。
优选地，步骤四中，建立昆虫构型足式机器人基节、股节和胫节的转动速度变量分别wKz，wHz，wAz：
Figure BDA0003110308900000061
Figure BDA0003110308900000062
Figure BDA0003110308900000063
式中wAx,wAy,wAz分别为基节沿牵引、侧向和法向的转动速度变量；wHx,wHy,wHz分别为股节沿牵引、侧向和法向的转动速度变量；wKx,wKy,wKz分别为胫节沿牵引、侧向和法向的转动速度变量。
根据股节和胫节的转动速度，建立昆虫构型足式机器人足地滑移状态变量方程：
Figure BDA0003110308900000064
式中
Figure BDA0003110308900000066
为足地滑移速度变量，fs(xs,ws1)为足地滑移加速度变量。
最终得到昆虫构型足式机器人的足端滑移加速度为：
Figure BDA0003110308900000065
式中FT为足地牵引向力参数，MA为基节连杆的质量，LA为基节长度；MH为大腿连杆的质量，LH为大腿长度；MK为小腿连杆的质量，LK为小腿长度，qA为髋关节角度，qH为髋关节角度，qK为膝关节角度。
昆虫构型足式机器人的足端滑移速度解算为：
Figure BDA0003110308900000071
式中，
Figure BDA0003110308900000072
为足地速度矢量，
Figure BDA0003110308900000073
为状态估计得到的基节侧向速度。
昆虫构型足式机器人的足端滑移位置解算为：
Figure BDA0003110308900000074
式中，LPsx为足地位置矢量，qA0为基关节初始角度，qK0为膝关节初始角度，qH0为髋关节初始角度，
Figure BDA0003110308900000075
为状态估计得到的基节的侧向速度，Δt为时间周期变量。
根据式(12)，(13)，(14)最终得到昆虫构型足式机器人的滑移状态方程为：
Figure BDA0003110308900000076
式中wsx为白噪声矩阵，协方差矩阵为Qsx。
基于扩展卡尔曼滤波的循环迭代方法对两种测量数据的信息进行融合得到足地滑移估计值ps(xs,ws3)，如图4所示，定义昆虫构型足式机器人牵引方向的足地滑移率SI(t)为：
Figure BDA0003110308900000077
LPxei为控制周期结束时，由正运动学计算出的足端在机器人坐标系下的位置；LPxsi为控制周期开始时，由正运动学计算出的足端在机器人坐标系下的位置。
当定义的滑移率处于SI(t)∈[0,1)这个范围内时，证明昆虫构型足式机器人的运动过程为有效运动。
优选地，步骤五基于步骤一至步骤四，进行昆虫构型足式机器人的足地滑移检测具体为：
步骤1：昆虫构型足式机器人的足地滑移检测起始于足端落地时刻，通过足力传感器检测落地时刻；
步骤2：利用步骤1足力传感器检测到的足地落地时刻状态，进行支撑相和摆动相的切换，通过昆虫构型足式机器人本体传感数据解算得到惯导和运动学状态变量；
步骤3：利用步骤2计算得到的惯导和运动学状态变量，作为状态估计框架中的输入变量，通过基于扩展卡尔曼滤波的昆虫构型足式机器人状态估计框架解算，基于惯导、运动学及足力数据融合得到的昆虫构型足式机器人位置、速度状态变量；
步骤4：利用步骤3计算得到的位置、速度状态变量，作为滑移估计框架中的输入变量，通过昆虫构型机器人足地滑移估计模型解算，建立昆虫构型足端牵引力方向滑移加速度模型；
步骤5：利用步骤4计算得到的滑移加速度模型，作为足地滑移状态估计的判定基础，通过基于扩展卡尔曼滤波的昆虫构型足地滑移状态估计方法，建立昆虫构型足式机器人的足地滑移率检测条件，检测足地滑移情况；
步骤6：利用步骤5判定得到的足地滑移情况，通过足力传感器检测判断支撑相是否结束，当支撑相结束后通过步态切换转变成摆动相。
本发明还提供了一种所述方法在足式机器人技术领域中的应用。
(三)有益效果
通过对一种电驱动昆虫构型足式机器人足地滑移估计模型及方法的研究，使得本发明所提昆虫构型足式机器人在牵引力方向移动过程中具有滑移状态实时检测功能，并可以通过检测到的足地滑移量实现足地滑移抑制功能，并提高足式机器人控制的运动稳定性。
附图说明
图1是本发明提供的一种电驱动昆虫构型足式机器人足地滑移估计方法流程图；
图2是本发明提供的昆虫构型足式机器人的牵引力方向机器人本体运动示意图；
图3是本发明提供的昆虫构型足式机器人的单腿运动示意图；
图4是本发明提供的昆虫构型足式机器人的足地滑移状态估计框图；
图5是本发明提供的昆虫构型足式机器人的足地滑移检测流程图。
具体实施方式
为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。
如图1所示，本发明提供的一种电驱动昆虫构型足式机器人足地滑移估计模型及方法具体是按照以下步骤完成的：
步骤一、设计昆虫构型足式机器人本体传感数据解算方法，得到机器人在导航坐标系中的加速度；
通过对惯导等传感器的数据进行标定及补偿，通过标定加速度及角加速度信号来抑制昆虫构型牵引力运动方向速度和姿态解算的误差，最终输出准确的昆虫构型机器人牵引力方向本体速度及三维姿态信息，惯导传感实时数据具体解算结果为：
Figure BDA0003110308900000101
式中，
Figure BDA0003110308900000102
为导航坐标系到机体坐标系的转换矩阵，fB为空间中的真实加速度和重力导致的加速度之差的力矢量，gn为重力矢量，
Figure BDA0003110308900000103
为地球自转角度在导航坐标系中的投影，
Figure BDA0003110308900000104
为机器人在导航坐标系中的速度，vn为机器人在导航坐标系中的速度。
步骤二、利用步骤一验证得到的机器人在导航坐标系中的加速度
Figure BDA0003110308900000105
作为状态估计框架中的输入变量
Figure BDA0003110308900000106
从而建立基于扩展卡尔曼滤波的昆虫构型足式机器人状态估计框架，得到昆虫构型足式机器人位置、速度估计变量；
根据足式机器人单腿及质心状态变量，建立状态估计过程中的预测模型，其具体建模结果为：
Figure BDA0003110308900000107
式中，f为三维方向的加速度真实值；bf为加速度计的偏差；bw为角加速度计的偏差；wf为加速度计的随机误差；ww为角加速度计的随机误差；w为三维方向的角速率真实值；
Figure BDA0003110308900000108
为三维方向的加速度测量值；
Figure BDA0003110308900000109
为三维方向的角速率测量值；wbf为加速度计噪声矩阵；wbw为角加速度计噪声矩阵。
建立状态估计过程中的测量模型，其具体建模结果为：
Figure BDA0003110308900000111
式中，pc为机器人的位置修正量；vc为机体的速度修正量；p为机器人的位置测量变量；v为机体的速度测量变量。
根据状态变量的预测更新和测量更新，基于反馈校正方法得到足式机器人的状态变量结果如下所示：
Figure BDA0003110308900000112
式中，
Figure BDA0003110308900000113
为位置估计值，
Figure BDA0003110308900000114
为速度估计值，Δt为时间周期变量；
Figure BDA0003110308900000115
为三维方向的加速度测量值；wf为加速度的偏差值，
Figure BDA0003110308900000116
为导航坐标系转换矩阵。
步骤三、利用步骤二公式(4)计算得到的昆虫构型足式机器人位置、速度估计变量，用于作为单腿动力学方程的输入变量，建立昆虫构型足式机器人足地滑移估计模型，得到的昆虫构型足式机器人足地滑移加速度模型；
建立足式机器人单腿滑移的动力学模型，根据昆虫构型足式机器人行走时的单腿滑移模型，昆虫构型足式机器人机体运动方式如图2所示，昆虫构型机器人单腿滑移运动方式如图3所示，两连杆的位置矢量可以由下式得到：
Figure BDA0003110308900000121
式中，LPsx为牵引方向滑移距离，LPsz为法向方向滑移距离，MA为基节连杆的质量，LA为基节长度；MH为大腿连杆的质量，LH为大腿长度；MK为小腿连杆的质量，LK为小腿长度，qA为髋关节角度，qH为髋关节角度，qK为膝关节角度。
定义滑移状态下的动力学系统输入变量为q＝[qA qK qH LPsx LPsz]T，足-地接触力定义为F＝[FT FN]T，FT代表足端所受到的切向力，FN代表足端所受到的法向力。
基于拉格朗日方程，建立单腿三自由度的运动动力学方程，如下所示：
Figure BDA0003110308900000122
式中，Dd(q)为惯性矩阵，
Figure BDA0003110308900000123
为哥氏力矩阵，Gd(q)为重力矩阵，Cd为重力矩阵，q为关节角度，
Figure BDA0003110308900000124
为关节角速度，
Figure BDA0003110308900000125
为关节角加速度，u为重力矩阵，Ed为重力矩阵，F为足力矢量。
根据单腿滑移动力学运动方程(6)，建立昆虫构型足端牵引力方向滑移加速度模型，如下所示：
Figure BDA0003110308900000126
式中FT为足地牵引向力参数，MA为基节连杆的质量，LA为基节长度；MH为大腿连杆的质量，LH为大腿长度；MK为小腿连杆的质量，LK为小腿长度，qA为髋关节角度，qH为髋关节角度，qK为膝关节角度。
步骤四、利用步骤三计算得到的昆虫构型足式机器人足地滑移加速度模型(7)，建立基于扩展卡尔曼滤波的昆虫构型足地滑移状态估计方法；
建立昆虫构型足式机器人基节、股节和胫节的转动速度变量分别wKz，wHz，wAz：
Figure BDA0003110308900000131
Figure BDA0003110308900000132
Figure BDA0003110308900000133
式中wAx,wAy,wAz分别为基节沿牵引、侧向和法向的转动速度变量；wHx,wHy,wHz分别为股节沿牵引、侧向和法向的转动速度变量；wKx,wKy,wKz分别为胫节沿牵引、侧向和法向的转动速度变量。
根据股节和胫节的转动速度，建立昆虫构型足式机器人足地滑移状态变量方程：
Figure BDA0003110308900000134
式中
Figure BDA0003110308900000135
为足地滑移速度变量，fs(xs,ws1)为足地滑移加速度变量。
最终得到昆虫构型足式机器人的足端滑移加速度为：
Figure BDA0003110308900000141
式中FT为足地牵引向力参数，MA为基节连杆的质量，LA为基节长度；MH为大腿连杆的质量，LH为大腿长度；MK为小腿连杆的质量，LK为小腿长度，qA为髋关节角度，qH为髋关节角度，qK为膝关节角度。
昆虫构型足式机器人的足端滑移速度解算为：
Figure BDA0003110308900000142
式中，
Figure BDA0003110308900000143
为足地速度矢量，
Figure BDA0003110308900000144
为状态估计得到的基节侧向速度。
昆虫构型足式机器人的足端滑移位置解算为：
Figure BDA0003110308900000145
式中，LPsx为足地位置矢量，qA0为基关节初始角度，qK0为膝关节初始角度，qH0为髋关节初始角度，
Figure BDA0003110308900000147
为状态估计得到的基节的侧向速度，Δt为时间周期变量。
根据式(12)，(13)，(14)最终得到昆虫构型足式机器人的滑移状态方程为：
Figure BDA0003110308900000146
式中wsx为白噪声矩阵，协方差矩阵为Qsx。
基于扩展卡尔曼滤波的循环迭代方法对两种测量数据的信息进行融合得到足地滑移估计值ps(xs,ws3)，如图4所示，定义昆虫构型足式机器人牵引方向的足地滑移率SI(t)为：
Figure BDA0003110308900000151
LPxei为控制周期结束时，由正运动学计算出的足端在机器人坐标系下的位置；LPxsi为控制周期开始时，由正运动学计算出的足端在机器人坐标系下的位置。
当定义的滑移率处于SI(t)∈[0,1)这个范围内时，证明昆虫构型足式机器人的运动过程为有效运动。
步骤五、基于步骤一至步骤四，进行昆虫构型足式机器人的足地滑移检测，如图5所示：
步骤1：昆虫构型足式机器人的足地滑移检测起始于足端落地时刻，通过足力传感器检测落地时刻；
步骤2：利用步骤1足力传感器检测到的足地落地时刻状态，进行支撑相和摆动相的切换，通过昆虫构型足式机器人本体传感数据解算得到惯导和运动学状态变量；
步骤3：利用步骤2计算得到的惯导和运动学状态变量，作为状态估计框架中的输入变量，通过基于扩展卡尔曼滤波的昆虫构型足式机器人状态估计框架解算，基于惯导、运动学及足力数据融合得到的昆虫构型足式机器人位置、速度状态变量；
步骤4：利用步骤3计算得到的位置、速度状态变量，作为滑移估计框架中的输入变量，通过昆虫构型机器人足地滑移估计模型解算，建立昆虫构型足端牵引力方向滑移加速度模型；
步骤5：利用步骤4计算得到的滑移加速度模型，作为足地滑移状态估计的判定基础，通过基于扩展卡尔曼滤波的昆虫构型足地滑移状态估计方法，建立昆虫构型足式机器人的足地滑移率检测条件，检测足地滑移情况；
步骤6：利用步骤5判定得到的足地滑移情况，通过足力传感器检测判断支撑相是否结束，当支撑相结束后通过步态切换转变成摆动相。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (10)

1.一种电驱动昆虫构型足式机器人足地滑移估计方法，其特征在于，包括以下步骤：
步骤一、设计昆虫构型足式机器人本体传感数据解算方法，得到机器人在导航坐标系中的加速度；
步骤二、利用步骤一验证得到的机器人在导航坐标系中的加速度，作为状态估计框架中的输入变量，从而建立基于扩展卡尔曼滤波的昆虫构型足式机器人状态估计框架，得到昆虫构型足式机器人位置、速度估计变量；
步骤三、利用步骤二计算得到的昆虫构型足式机器人位置、速度估计变量，用于作为单腿动力学方程的输入变量，建立昆虫构型足式机器人足地滑移估计模型，得到的昆虫构型足式机器人足地滑移加速度模型；
步骤四、利用步骤三计算得到的昆虫构型足式机器人足地滑移加速度模型，建立基于扩展卡尔曼滤波的昆虫构型足地滑移状态估计方法；
步骤五、基于步骤一至步骤四，进行昆虫构型足式机器人的足地滑移检测。
2.如权利要求1所述的方法，其特征在于，步骤一中，通过对惯导等传感器的数据进行标定及补偿，通过标定加速度及角加速度信号来抑制昆虫构型牵引力运动方向速度和姿态解算的误差，最终输出准确的昆虫构型机器人牵引力方向本体速度及三维姿态信息，惯导传感实时数据具体解算结果为：
Figure FDA0003110308890000021
式中，
Figure FDA0003110308890000022
为导航坐标系到机体坐标系的转换矩阵，fB为空间中的真实加速度和重力导致的加速度之差的力矢量，gn为重力矢量，
Figure FDA0003110308890000023
为地球自转角度在导航坐标系中的投影，
Figure FDA0003110308890000024
为机器人在导航坐标系中的速度，vn为机器人在导航坐标系中的速度。
3.如权利要求2所述的方法，其特征在于，步骤二中，利用步骤一验证得到的机器人在导航坐标系中的加速度
Figure FDA0003110308890000025
作为状态估计框架中的输入变量
Figure FDA0003110308890000026
从而建立基于扩展卡尔曼滤波的昆虫构型足式机器人状态估计框架，得到昆虫构型足式机器人位置、速度估计变量。
4.如权利要求3所述的方法，其特征在于，步骤二中，根据足式机器人单腿及质心状态变量，建立状态估计过程中的预测模型，其具体建模结果为：
Figure FDA0003110308890000027
式中，f为三维方向的加速度真实值；bf为加速度计的偏差；bw为角加速度计的偏差；wf为加速度计的随机误差；ww为角加速度计的随机误差；w为三维方向的角速率真实值；
Figure FDA0003110308890000028
为三维方向的加速度测量值；
Figure FDA0003110308890000029
为三维方向的角速率测量值；wbf为加速度计噪声矩阵；wbw为角加速度计噪声矩阵。
建立状态估计过程中的测量模型，其具体建模结果为：
Figure FDA0003110308890000031
式中，pc为机器人的位置修正量；vc为机体的速度修正量；p为机器人的位置测量变量；v为机体的速度测量变量。
根据状态变量的预测更新和测量更新，基于反馈校正方法得到足式机器人的状态变量结果如下所示：
Figure FDA0003110308890000032
式中，
Figure FDA0003110308890000033
为位置估计值，
Figure FDA0003110308890000034
为速度估计值，Δt为时间周期变量；
Figure FDA0003110308890000035
为三维方向的加速度测量值；wf为加速度的偏差值，
Figure FDA0003110308890000036
为导航坐标系转换矩阵。
5.如权利要求4所述的方法，其特征在于，步骤三中，利用步骤二公式(4)计算得到的昆虫构型足式机器人位置、速度估计变量，用于作为单腿动力学方程的输入变量，建立昆虫构型足式机器人足地滑移估计模型，得到的昆虫构型足式机器人足地滑移加速度模型。
6.如权利要求5所述的方法，其特征在于，步骤三中，建立足式机器人单腿滑移的动力学模型，根据昆虫构型足式机器人行走时的单腿滑移模型，得到两连杆的位置矢量：
Figure FDA0003110308890000037
式中，LPsx为牵引方向滑移距离，LPsz为法向方向滑移距离，MA为基节连杆的质量，LA为基节长度；MH为大腿连杆的质量，LH为大腿长度；MK为小腿连杆的质量，LK为小腿长度，qA为髋关节角度，qH为髋关节角度，qK为膝关节角度；
定义滑移状态下的动力学系统输入变量为q＝[qA qK qH LPsx LPsz]T，足-地接触力定义为F＝[FT FN]T，FT代表足端所受到的切向力，FN代表足端所受到的法向力；
基于拉格朗日方程，建立单腿三自由度的运动动力学方程，如下所示：
Figure FDA0003110308890000041
式中，Dd(q)为惯性矩阵，
Figure FDA0003110308890000042
为哥氏力矩阵，Gd(q)为重力矩阵，Cd为重力矩阵，q为关节角度，
Figure FDA0003110308890000043
为关节角速度，
Figure FDA0003110308890000044
为关节角加速度，u为重力矩阵，Ed为重力矩阵，F为足力矢量；
根据单腿滑移动力学运动方程(6)，建立昆虫构型足端牵引力方向滑移加速度模型，如下所示：
Figure FDA0003110308890000045
式中FT为足地牵引向力参数，MA为基节连杆的质量，LA为基节长度；MH为大腿连杆的质量，LH为大腿长度；MK为小腿连杆的质量，LK为小腿长度，qA为髋关节角度，qH为髋关节角度，qK为膝关节角度。
7.如权利要求6所述的方法，其特征在于，步骤四中，利用步骤三计算得到的昆虫构型足式机器人足地滑移加速度模型(7)，建立基于扩展卡尔曼滤波的昆虫构型足地滑移状态估计方法。
8.如权利要求7所述的方法，其特征在于，步骤四中，建立昆虫构型足式机器人基节、股节和胫节的转动速度变量分别wKz，wHz，wAz：
Figure FDA0003110308890000051
Figure FDA0003110308890000052
Figure FDA0003110308890000053
式中wAx,wAy,wAz分别为基节沿牵引、侧向和法向的转动速度变量；wHx,wHy,wHz分别为股节沿牵引、侧向和法向的转动速度变量；wKx,wKy,wKz分别为胫节沿牵引、侧向和法向的转动速度变量。
根据股节和胫节的转动速度，建立昆虫构型足式机器人足地滑移状态变量方程：
Figure FDA0003110308890000054
式中
Figure FDA0003110308890000055
为足地滑移速度变量，fs(xs,ws1)为足地滑移加速度变量。
最终得到昆虫构型足式机器人的足端滑移加速度为：
Figure FDA0003110308890000061
式中FT为足地牵引向力参数，MA为基节连杆的质量，LA为基节长度；MH为大腿连杆的质量，LH为大腿长度；MK为小腿连杆的质量，LK为小腿长度，qA为髋关节角度，qH为髋关节角度，qK为膝关节角度。
昆虫构型足式机器人的足端滑移速度解算为：
Figure FDA0003110308890000062
式中，
Figure FDA0003110308890000063
为足地速度矢量，
Figure FDA0003110308890000064
为状态估计得到的基节侧向速度。
昆虫构型足式机器人的足端滑移位置解算为：
Figure FDA0003110308890000065
式中，LPsx为足地位置矢量，qA0为基关节初始角度，qK0为膝关节初始角度，qH0为髋关节初始角度，
Figure FDA0003110308890000066
为状态估计得到的基节的侧向速度，Δt为时间周期变量。
根据式(12)，(13)，(14)最终得到昆虫构型足式机器人的滑移状态方程为：
Figure FDA0003110308890000067
式中wsx为白噪声矩阵，协方差矩阵为Qsx。
基于扩展卡尔曼滤波的循环迭代方法对两种测量数据的信息进行融合得到足地滑移估计值ps(xs,ws3)，如图4所示，定义昆虫构型足式机器人牵引方向的足地滑移率SI(t)为：
Figure FDA0003110308890000071
LPxei为控制周期结束时，由正运动学计算出的足端在机器人坐标系下的位置；LPxsi为控制周期开始时，由正运动学计算出的足端在机器人坐标系下的位置。
当定义的滑移率处于SI(t)∈[0,1)这个范围内时，证明昆虫构型足式机器人的运动过程为有效运动。
9.如权利要求8所述的方法，其特征在于，步骤五基于步骤一至步骤四，进行昆虫构型足式机器人的足地滑移检测具体为：
步骤1：昆虫构型足式机器人的足地滑移检测起始于足端落地时刻，通过足力传感器检测落地时刻；
步骤2：利用步骤1足力传感器检测到的足地落地时刻状态，进行支撑相和摆动相的切换，通过昆虫构型足式机器人本体传感数据解算得到惯导和运动学状态变量；
步骤3：利用步骤2计算得到的惯导和运动学状态变量，作为状态估计框架中的输入变量，通过基于扩展卡尔曼滤波的昆虫构型足式机器人状态估计框架解算，基于惯导、运动学及足力数据融合得到的昆虫构型足式机器人位置、速度状态变量；
步骤4：利用步骤3计算得到的位置、速度状态变量，作为滑移估计框架中的输入变量，通过昆虫构型机器人足地滑移估计模型解算，建立昆虫构型足端牵引力方向滑移加速度模型；
步骤5：利用步骤4计算得到的滑移加速度模型，作为足地滑移状态估计的判定基础，通过基于扩展卡尔曼滤波的昆虫构型足地滑移状态估计方法，建立昆虫构型足式机器人的足地滑移率检测条件，检测足地滑移情况；
步骤6：利用步骤5判定得到的足地滑移情况，通过足力传感器检测判断支撑相是否结束，当支撑相结束后通过步态切换转变成摆动相。
10.一种如权利要求1至9中任一项所述方法在足式机器人技术领域中的应用。
CN202110647027.XA 2021-06-10 2021-06-10 一种电驱动昆虫构型足式机器人足地滑移估计方法 Pending CN113358121A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110647027.XA CN113358121A (zh) 2021-06-10 2021-06-10 一种电驱动昆虫构型足式机器人足地滑移估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110647027.XA CN113358121A (zh) 2021-06-10 2021-06-10 一种电驱动昆虫构型足式机器人足地滑移估计方法

Publications (1)

Publication Number Publication Date
CN113358121A true CN113358121A (zh) 2021-09-07

Family

ID=77533794

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110647027.XA Pending CN113358121A (zh) 2021-06-10 2021-06-10 一种电驱动昆虫构型足式机器人足地滑移估计方法

Country Status (1)

Country Link
CN (1) CN113358121A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115407790A (zh) * 2022-08-16 2022-11-29 中国北方车辆研究所 一种基于深度学习的四足机器人侧向速度估算方法

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
刘宇飞: "面向非预知地形的六足机器人足力优化及滑移抑制研究", 《哈尔滨工业大学博士学位论文》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115407790A (zh) * 2022-08-16 2022-11-29 中国北方车辆研究所 一种基于深度学习的四足机器人侧向速度估算方法
CN115407790B (zh) * 2022-08-16 2024-04-26 中国北方车辆研究所 一种基于深度学习的四足机器人侧向速度估算方法

Similar Documents

Publication Publication Date Title
CN102121828B (zh) 一种仿人机器人身体姿态角度实时估计方法
CN106547206B (zh) 一种基于脚力估计算法的多足机器人动态稳定性定量判定方法
CN108621161B (zh) 基于多传感器信息融合的足式机器人本体状态估计方法
CN109634100B (zh) 仿人机器人行走加速度补偿方法、装置及仿人机器人
EP3186041B1 (en) Natural pitch and roll
CN110146077A (zh) 移动机器人姿态角解算方法
KR101665543B1 (ko) 인간형 로봇의 안정화 장치 및 그 방법
US7145305B2 (en) System for estimating attitude of leg type moving robot itself
Löffler et al. Sensors and control concept of walking “Johnnie”
US20060106495A1 (en) Control device of legged mobile robot
US7561941B2 (en) Ambulatory robot and method for controlling the same
CN108061855A (zh) 一种基于mems传感器的球形电机转子位置检测方法
Gonzalez et al. Line walking and balancing for legged robots with point feet
Käslin et al. Towards a passive adaptive planar foot with ground orientation and contact force sensing for legged robots
CN110450159A (zh) 一种双足机器人的足部状态检测装置以及基于该装置的斜面行走方法
CN113031579A (zh) 双足机器人的行走控制方法、装置及双足机器人
JP2016064454A (ja) 移動ロボットの制御装置
CN113126659A (zh) 一种仿人机器人跳跃着地状态检测系统与方法
CN113358121A (zh) 一种电驱动昆虫构型足式机器人足地滑移估计方法
Masuya et al. Com motion estimation of a humanoid robot based on a fusion of dynamics and kinematics information
CN112757301B (zh) 机器人抗扰动控制方法、装置、电子设备及存储介质
CN210757742U (zh) 一种双足机器人的足部状态检测装置
JP2021091064A (ja) 外乱オブザーバ及び外乱オブザーバを用いた接触点推定手段
JP2011255500A (ja) 脚式移動ロボットの制御装置
CN113405548A (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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20210907