CN114995476A - 一种步行机器人坡地崎岖地形的步态生成与姿态控制方法 - Google Patents

一种步行机器人坡地崎岖地形的步态生成与姿态控制方法 Download PDF

Info

Publication number
CN114995476A
CN114995476A CN202210526150.0A CN202210526150A CN114995476A CN 114995476 A CN114995476 A CN 114995476A CN 202210526150 A CN202210526150 A CN 202210526150A CN 114995476 A CN114995476 A CN 114995476A
Authority
CN
China
Prior art keywords
coordinate system
legs
angle
group
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.)
Granted
Application number
CN202210526150.0A
Other languages
English (en)
Other versions
CN114995476B (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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202210526150.0A priority Critical patent/CN114995476B/zh
Publication of CN114995476A publication Critical patent/CN114995476A/zh
Application granted granted Critical
Publication of CN114995476B publication Critical patent/CN114995476B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种步行机器人坡地崎岖地形的步态生成与姿态控制方法。本发明步行机器人坡地崎岖地形的步态生成与姿态控制方法，在经典三角步态的基础上引入足端力触地反馈，作为步态判断与切换的依据，只有当摆动腿触地之后才进行支撑相轨迹规划，且只有当处于摆动的一组主支撑腿完全落地支撑后，当前支撑的另一组主支撑腿才进入摆动相轨迹规划，确保机器人在连续行走的过程中始终有不少于三条腿支撑，平稳性大大提高。本发明步行机器人坡地崎岖地形的步态生成与姿态控制方法灵活性高且地形适应性强。

Description

一种步行机器人坡地崎岖地形的步态生成与姿态控制方法
技术领域
本发明涉及多足步行机器人运动规划技术领域，特别是涉及一种步行机器人坡地崎岖地形的步态生成与姿态控制方法。
背景技术
足式机器人足端与地面间的接触是离散点接触方式，相比于轮式、履带式机器人具有更强的崎岖地形适应性，复杂地形上的运动性能优势是足式机器人未来发展的潜力。
目前的足式机器人行走普遍采用支撑相与摆动相相互交替的方式，这与自然界中的动物类似。位于支撑相的足端支撑住身体并按照期望的轨迹运动，根据运动的相对性，身体与地面间会产生位移，位于摆动相的足端在空中划出一个弧线落在下一个支撑的位置，如此摆动相与支撑相往复交替，机器人才能够连续不断地行走。
三角步态是六足机器人常用的步态，但传统的三角步态规划不考虑地形的真实情况，摆动腿同一时间进入支撑，支撑腿同一时间进入摆动，导致在步态切换时，常常出现足端悬空的情况，造成机器人行走不平稳。
机器人坡地步态的研究是实现机器人崎岖地形行走必不可少的一部分，目前多数的研究都针对机器人完全位于坡面上的情况，不考虑机器人由水平地面爬上坡面这种地形交界处步态规划的情况，很难实现机器人在不同地形上的连续行走。在坡地地形行走时，姿态角的规划与控制能够提高机器人的地形适应性，目前使用的控制算法，如CPG中枢模式发生器模型，在四足机器人上应用较多，但比较少研究横滚角的规划与控制情况(Zhuo Ge,Qingsheng Luo,Baoling Han,Qi Na,Huashi Li.Slope Terrain Locomotion Control ofa Quadruped Robot Based on Biological Reflex CPG Model.Journal of BeijingInstitute of Technology,2018,27(2):257-266)，基于经验的模糊控制方法([1]杜威，杨忠，许昌亮，张辉斌.四足机器人斜面运动规划及控制研究[J].电气与自动化,2018,6:150-153.[2]俞文雅,陶红武,曾顺,谭跃刚.四足机器人斜坡对角小跑运动控制研究[J].武汉科技大学学报,2021,44(1):60-67)，则不能够给出明确的姿态角解算方式，导致姿态角的控制不够灵活。因此，提出一种适用于六足机器人或六足以上的多足机器人的坡地地形下的连续步态规划与姿态角控制的方法是很有必要的。
发明内容
本发明的目的是为了实现多足步行机器人位于坡地崎岖地形时的连续步态生成以及实时姿态控制，使得机器人更好地适应崎岖地形并提高行走的稳定性。本发明不考虑不可落足地形或者具有不可跨越障碍物的地形。
一种步行机器人坡地崎岖地形的步态生成与姿态控制方法，所述步行机器人包括机身和至少6条腿，将机身一周的所有腿中相互间隔的腿分为同一组，共可分为两组，两组腿各称为一组主支撑腿，每条腿有支撑相和摆动相两种状态；同一组的各条腿均处于支撑相时，该组各条腿的足端所构成的平面为主支撑面，
定义机身坐标系{XB,YB,ZB}、世界坐标系和主支撑面坐标系{XS,YS,ZS}，机身坐标系原点OB位于机身形心位置，过机身坐标系原点作主支撑面的垂线，垂足为主支撑面坐标系原点OS，ZS方向为主支撑面法向，XS的方向与XW沿重力方向在主支撑面上的投影轴方向一致，将OB到主支撑面的垂直距离定义为机身高度H，
所述步态生成与姿态控制方法包括以下步骤：
(1)人为给出行走速度和行走方向指令后，将处于支撑相的一组腿称为第一组腿，另一组腿称为第二组腿，分析处于支撑相的第一组腿所构成的主支撑面信息，规划机身坐标系相对于世界坐标系的移动速度指令
Figure BDA0003644438830000021
和姿态角指令Ee，用来区分哪一组腿支撑的标识符LR置0，处于支撑相的第一组腿的关节角度使用支撑相的步态规划，第二组腿的关节角度使用摆动相的步态规划；
(2)将Ee经过PID运算后得到的值作为姿态角速度指令
Figure BDA0003644438830000022
将
Figure BDA0003644438830000023
和
Figure BDA0003644438830000024
共同输入速度逆运动学进行计算，得到关节角速度指令
Figure BDA0003644438830000025
经过积分得到关节角度指令θi，i为腿的编号，作为处于支撑相的第一组腿的关节驱动量；将第二组腿的足端初始位置作为摆动轨迹的起始位置，采用六次插值方式规划摆动腿足端轨迹，经过腿部几何逆解，得到关节角度指令θi，i为腿的编号，作为处于摆动相的第二组腿的关节驱动量；
(3)第二组腿在摆动的过程中，陆续触地，在触地的瞬间进入支撑相步态规划，将触地瞬间的关节角速度作为积分初值，通过速度逆运动学方式得出关节角速度，积分得到关节驱动量，与原先支撑的第一组腿一起，共同支撑机身。
(4)在第二组腿都触地之后，分析第二组腿所构成的主支撑面信息，重新规划机身坐标系相对于世界坐标系的移动速度指令
Figure BDA0003644438830000031
和姿态角指令Ee，标识符LR置1，第二组腿继续支撑，第一组腿开始摆动，规划方法同步骤(2)；
(5)第一组腿在摆动的过程中，陆续触地，在触地的瞬间进入支撑相步态规划，规划方法同步骤(3)。
(6)在第一组腿都触地之后，回到步骤(1)，完成一个步态周期，重复步骤(1)至(5)的过程，步态进行循环，步行机器人在腿的不断支撑与摆动交替的过程中实现坡地崎岖地形上的连续行走。
其中说的机身一周的所有腿中相互间隔的腿分为同一组，对于机身是偏圆形的，则机身一周较好理解；如果机身是长方形的，所有腿分布在机身两侧，则两侧中都位于头部的两条腿，按机身一周来说，就属于相邻的两条腿，同样的，两侧中都位于尾部的两条腿也属于相邻的两条腿。
优选的，步骤(1)中，当第一组腿触地后，确定主支撑面坐标系，该主支撑面坐标系在世界坐标系下的姿态可以表示为：
Figure BDA0003644438830000032
其中，
Figure BDA0003644438830000033
均是3×1的列向量，
Figure BDA0003644438830000034
表示XS的单位向量在世界坐标系的三个轴上的投影值，
Figure BDA0003644438830000035
表示YS的单位向量在世界坐标系的三个轴上的投影值，
Figure BDA0003644438830000036
表示ZS的单位向量在世界坐标系的三个轴上的投影值。
Figure BDA0003644438830000037
的计算方法如下：
Figure BDA0003644438830000041
其中，WPi(i＝1～6)表示各条腿的足端在世界坐标系下的坐标位置，通过机器人正运动学和经典的坐标变换可以得到，上标“→”表示是由足端点组成的向量，“|| ||”表示取向量模长，
由机器人正运动学可以得到的各条腿足端在机身坐标系下的坐标位置BPi，由处于支撑相的一组腿的3个足端坐标可以计算得到主支撑面在机身坐标系下的平面方程，并由机身坐标系原点到平面的距离公式确定机身高度H。
更优选的，步骤(1)和(4)中，第一组腿和第二组腿在相互切换支撑和摆动的过程中，由于主支撑面的改变，都会重新产生一个目标姿态角，相邻的两个目标姿态角之间需要通过插值的方式规划出一条轨迹值，作为姿态角指令Ee。
目标值是单个的值，在步态切换的时候才得到的，期望值是指令值，是一串指令，是在目标值之间插值得到的。
进一步优选的，期望机身与主支撑面平行时，需要使ZB与ZS平行，ZS的单位向量在世界坐标系中的表达式为
Figure BDA0003644438830000042
中的
Figure BDA0003644438830000043
解算出机身姿态角的目标值即为目标姿态角，具体算法如下：
Figure BDA0003644438830000051
Figure BDA0003644438830000052
表示机身坐标系相对于世界坐标系的目标旋转矩阵，Rot(z,αe)表示绕世界坐标系z轴旋转αe角，Rot(y,βe)表示绕世界坐标系y轴旋转βe角，Rot(x,γe)表示绕世界坐标系x轴旋转γe角，
αe：目标偏航角，βe：目标俯仰角，γe：目标横滚角，s和c分别为三角函数sin和cos的简写，
令
Figure BDA0003644438830000053
直行时αe＝0，因此，
Figure BDA0003644438830000054
可以列出两个独立方程，βe,γe∈[-90°,90°]范围内，求出βe、γe角的唯一解，
Figure BDA0003644438830000055
分别表示
Figure BDA0003644438830000056
向量的第1、2、3个值。
更优选的，步骤(1)中，移动速度指令
Figure BDA0003644438830000057
通过结合人为输入的行进速度指令
Figure BDA0003644438830000058
以及机身高度H得到，人为输入的行进速度是相对于主支撑面来说的，需要向世界坐标系投影，期望机身沿XS方向有一个速度
Figure BDA0003644438830000059
则该速度在世界坐标系的投影速度为
Figure BDA00036444388300000510
期望机身高度是一个定值He，则在主支撑面切换的时刻要对高度进行补偿，给出以一个沿ZS方向的速度
Figure BDA00036444388300000511
则该速度在世界坐标系的投影速度为
Figure BDA00036444388300000512
最终移动速度指令
Figure BDA00036444388300000513
优选的，步骤(2)中，在机器人行走过程中姿态角的实时控制时，引入机身上陀螺仪IMU的实时数据作为反馈，与规划的姿态角轨迹Ee作差，采用PID控制策略，将偏差值作为姿态角速度指令
Figure BDA00036444388300000514
引入机器人速度逆运动学模块进行计算
更优选的，机器人速度逆运动学模块中的机器人速度逆运动学是指已知机身相对于世界坐标系的运动速度指令，包括三个方向的移动速度和三个方向的转动速度，然后得出支撑足端相对于机身的速度，再通过雅克比矩阵求逆得出关节角速度，积分可得关节角度，从而计算执行器所需的位移量，驱动执行器，完成支撑相步态。
进一步优选的，关节角度指令θi的具体求解算法如下：
Figure BDA0003644438830000061
Figure BDA0003644438830000062
足端在世界坐标系下的移动速度，
Figure BDA0003644438830000063
足端在机身坐标系下的移动速度，
Figure BDA0003644438830000064
机身坐标系相对于世界坐标系的旋转变换矩阵，
Figure BDA0003644438830000065
机身坐标系相对于世界坐标系的旋转速度变换矩阵，
Figure BDA0003644438830000066
机身坐标系相对于世界坐标系的移动速度指令，
其中，
Figure BDA0003644438830000067
Rot(z,α)表示绕世界坐标系z轴旋转α角，Rot(y,β)表示绕世界坐标系y轴旋转β角，Rot(x,γ)表示绕世界坐标系x轴旋转γ角，
α：偏航角，β：俯仰角，γ：横滚角，s和c分别为三角函数sin和cos的简写，
Figure BDA0003644438830000068
Ji：i号腿的速度雅克比矩阵，
Figure BDA0003644438830000069
i号腿的各个关节角速度，L0：根关节与髋关节之间的连杆长度，L1：髋关节与膝关节之间的连杆长度，L2：膝关节与足端之间的连杆长度，θi0：i号腿的根关节旋转角度，θi1：i号腿的髋关节旋转角度，θi2：i号腿的膝关节旋转角度，
Figure BDA0003644438830000071
Figure BDA0003644438830000072
Figure BDA0003644438830000073
ωx、ωy、ωz分别是机身坐标系相对于世界坐标系的XW、YW、ZW三轴的旋转角速度，
Figure BDA0003644438830000074
即为期望姿态角与IMU反馈的实际姿态角之间的偏差信号经过PID运算后得到的姿态角速度指令
Figure BDA0003644438830000075
Figure BDA0003644438830000076
Figure BDA0003644438830000077
Figure BDA0003644438830000078
θi：i号腿计算执行器位移所需的三个关节角度值，θi_initial：单条腿在由摆动到触地成为支撑腿的时刻的三个关节角度的初始值，t：该腿在进入支撑相到结束支撑相的整个支撑时长。
优选的，单腿进入支撑相的条件是：摆动过程中检测到由足端力传感器反馈的触地信号；单腿进入摆动相的条件是：另一组主支撑腿由摆动相全部着地进入支撑相。
更优选的，单腿步态切换时的规划方式如下：在摆动相切换至支撑相时，将切换时刻的关节角度作为关节角速度积分的初始值，在支撑相切换至摆动相时，将切换时刻的足端位置作为足端轨迹的起点。
本发明的有益效果为：
本发明步行机器人坡地崎岖地形的步态生成与姿态控制方法，在经典三角步态的基础上引入足端力触地反馈，作为步态判断与切换的依据，只有当摆动腿触地之后才进行支撑相轨迹规划，且只有当处于摆动的一组主支撑腿完全落地支撑后，当前支撑的另一组主支撑腿才进入摆动相轨迹规划，确保机器人在连续行走的过程中始终有不少于三条腿支撑，平稳性大大提高。
支撑相轨迹与摆动相轨迹采用不同的规划方式，足端摆动时，在空中划过一个弧线落在下一个支撑点位置，摆动相的轨迹规划，正是对于该弧线的规划，摆动腿本身对机身的移动和姿态调整不起作用，机身的移动与姿态调整只与支撑腿有关，由于总有不少于三条腿支撑，支撑腿与机身之间构成了一个并联结构，规划支撑腿关节轨迹，就可以实现机身六个自由度方向上的期望运动。本发明中将机身IMU信号作负反馈，采用PID控制策略，同关节角度传感器反馈的角度值一起送入机器人速度逆运动学模型进行计算，得出关节角速度值，积分得到关节轨迹，实现机器人连续行走过程中实时姿态控制，姿态角的期望值则根据坡地地形得到。
坡地崎岖地形简化为由主支撑腿足端构成的主支撑面，假想机器人总是处于角度不同的平面之上，充分利用了足式机器人支撑足端离散的特点。由于主支撑面的确定只与主支撑腿的足端位置有关，因此，一组主支撑腿只要能够落地支撑，就可以确定该平面，无论坡地面是否平整，都是可以解算的。通过姿态控制，机器人机身可以保持水平、与主支撑面平行或与主支撑面成一角度，从机身轨迹角度出发给出指令，再进行各个腿的轨迹规划，由机器人速度逆运动学本身解算完成，不需要人为针对各种坡地角度重新规划，大大减少工作量。
综上，这是一种灵活性高且地形适应性强的步态生成和姿态控制方法。
附图说明
图1为机器人坡地崎岖地形下坐标系定义。
图2为六足机器人坡地崎岖地形总体控制框图。
图3为六足机器人坡地崎岖地形步态相位图。
图4为使用本发明所述的控制策略进行不同地形仿真的情况。
具体实施方式
本发明适用于六足及六足以上的多足机器人的步态规划，或者四足机器人采用爬行步态时的步态规划。以六足机器人为例，以下结合附图和具体实施例对本发明作进一步的说明：
图1是本发明中对于坐标系的定义，图中的机器人位于平地与坡面的交界位置，机器人的每条腿有三个关节：根关节，膝关节和髋关节。此时1、4、5号腿处于支撑相，2、3、6号腿处于摆动相，1、4、5号腿足端所构成的平面是主支撑面。图中有三个坐标系，分别为：机身坐标系{XB,YB,ZB}、世界坐标系{XW,YW,ZW}和主支撑面坐标系{XS,YS,ZS}。机身坐标系原点OB位于机身形心位置，过机身坐标系原点作主支撑面的垂线，垂足为主支撑面坐标系原点OS，ZS方向为主支撑面法向，XS的方向与XW沿重力方向在主支撑面上的投影轴方向一致，将OB到主支撑面的垂直距离(即OB和OS的连线长度)定义为机身高度H。
占空比是指一个步态周期中腿处于支撑相的时间所占的比例。步态周期是指单腿由摆动开始到下一次摆动或者由支撑开始到下一次支撑所用的时间，一个步态周期中包含了一个摆动相时间和一个支撑相时间。
图2是本发明中的步态相位图，对于六足机器人来说，按照占空比β的不同，常用的步态可分为三角
Figure BDA0003644438830000091
四角
Figure BDA0003644438830000092
五角
Figure BDA0003644438830000093
自由步态
Figure BDA0003644438830000094
等。本发明在三角步态的基础上进行了改进，步态形式上仍与三角步态类似，但由于地形的未知性，行走的过程摆动腿足端落地时间不一致，会有多于三条腿着地共同支撑机身的情况，因此是一种非典型的三角步态。
图2中黑色部分表示该腿处于支撑相，白色部分表示该腿处于摆动相。定义单腿从摆动相开始时刻到下一次摆动相开始时刻所经历的时间为一个步态周期T。将六条腿中相互间隔的腿分为同一组，即图1中1、4、5号腿一组，2、3、6号腿一组(即，同侧相互间隔的腿分为同一组，异侧位于同一排的两条腿分在不同组)，两组腿各称为一组主支撑腿，以标识符LR加以区分，LR的0值和1值分别对应一组主支撑腿。每个步态周期中只有同一组的三条腿同时摆动，当一组主支撑腿完全着地之后，另一组主支撑腿才可以进入摆动，因此，1、4、5号腿的步态周期(T1)一致，2、3、6号腿的步态周期(T2)一致。
图3是本发明的完整控制框图，包括了以下部分：
1.地形识别方法：当一组主支撑腿着地后，该组腿构成的主支撑面坐标系确定(按照图1的方法建立坐标系)，可以得到该主支撑面坐标系在世界坐标系下的姿态表达
Figure BDA0003644438830000095
Figure BDA0003644438830000101
其中，
Figure BDA0003644438830000102
均是3×1的列向量，
Figure BDA0003644438830000103
表示XS的单位向量在世界坐标系的三个轴上的投影值，
Figure BDA0003644438830000104
表示YS的单位向量在世界坐标系的三个轴上的投影值，
Figure BDA0003644438830000105
表示ZS的单位向量在世界坐标系的三个轴上的投影值。
Figure BDA0003644438830000106
的计算方法如下：
Figure BDA0003644438830000107
其中，WPi(i＝1～6)表示各条腿的足端在世界坐标系下的坐标位置，通过机器人正运动学和经典的坐标变换可以得到，上标“→”表示是由足端点组成的向量，“|| ||”表示取向量模长，
由机器人正运动学可以得到的各条腿足端在机身坐标系下的坐标位置BPi，由一组主支撑腿的3个足端坐标可以计算得到主支撑面在机身坐标系下的平面方程，并由机身原点到平面的距离公式确定机身高度H。
2.机身轨迹规划方法：机身轨迹规划的目的是为了使机身与主支撑面保持平行或成一夹角关系，并且提供一个行进的速度和行进的方向，这些都是机身坐标系相对于世界坐标系{XW,YW,ZW}而言的。机身的轨迹规划包括了姿态角度规划以及机身速度规划，假如期望机身与主支撑面平行，只需要使ZB与ZS平行，ZS的单位向量在世界坐标系中的表达式即为第1点提到的
Figure BDA0003644438830000108
中的
Figure BDA0003644438830000109
可以解算出机身姿态角的目标值，具体算法如下：
Figure BDA0003644438830000111
Rot(z,αe)表示绕世界坐标系z轴旋转αe角，Rot(y,βe)表示绕世界坐标系y轴旋转βe角，Rot(x,γe)表示绕世界坐标系x轴旋转γe角。
αe：目标偏航角，βe：目标俯仰角，γe：目标横滚角，本专利中使用s和c来简写三角函数sin和cos，下文中的公式中也同样使用简写形式。
令
Figure BDA0003644438830000112
直行时αe＝0，因此，
Figure BDA0003644438830000113
可以列出两个独立方程，求出βe、γe角的唯一解(βe,γe∈[-90°,90°])，
Figure BDA0003644438830000114
分别表示
Figure BDA0003644438830000115
向量的第1、2、3个值。
每次在切换主支撑腿的时刻(由一组主支撑腿支撑切换为另一组主支撑腿支撑)由于主支撑面的改变，都会重新产生一个目标姿态角，相邻的两个目标姿态角之间需要通过插值(可采用简单的线性插值)的方式规划出一条轨迹值，作为姿态角指令Ee。机身速度指令
Figure BDA0003644438830000116
则要结合人为输入的行进速度指令
Figure BDA0003644438830000117
以及机身高度H才能得到。人为输入的行进速度是相对于主支撑面而言的，还要将此速度向世界坐标系投影，例如，期望机身沿XS方向有一个速度
Figure BDA0003644438830000118
则该速度在世界坐标系的投影速度为
Figure BDA0003644438830000119
假设期望机身高度是一个定值He，那么在主支撑面切换(即由1、4、5号腿主要支撑切换为2、3、6号腿主要支撑或者由2、3、6号腿主要支撑切换为1、4、5号腿主要支撑)的时刻就要对高度进行补偿，给出以一个沿ZS方向的速度
Figure BDA00036444388300001110
则该速度在世界坐标系的投影速度为
Figure BDA0003644438830000121
最终速度指令
Figure BDA0003644438830000122
3.姿态控制方法：在机器人学中，机器人的姿态常用三个角度进行定义：偏航角(Yaw)、俯仰角(Pitch)、横滚角(Roll)。要实现姿态角的调整，需要至少3条腿着地，多于3条腿着地时，机身拥有冗余自由度。对于可以实现3条腿同时着地的机器人，都可以使用这种姿态控制算法，因此六足及六足以上机器人适用，四足机器人采用爬行步态时也适用。机器人的单条腿可以看作是一个简单的3自由度串联机械臂，着地腿与机身之间则组成了一个复杂的并联机构。为实现机器人行走过程中姿态角的实时控制，引入机身上陀螺仪IMU的实时数据作为反馈，与规划的姿态角轨迹作差，采用PID控制策略，将偏差值作为姿态角速度指令引入机器人速度逆运动学模块(在第4点中详细描述)进行计算，从而得到关节角轨迹。
4.步态生成方法：机器人足端步态分为支撑相步态和摆动相步态，两种步态有各自的规划方式。摆动项步态采用六次插值方式规划足端轨迹，再由单腿逆运动学求解得到关节轨迹曲线，使足端在空中划出一个曲线后落地。支撑相步态采用机器人速度逆运动学方式统一规划，处于支撑相的腿可以是3或4或5或6条。机器人速度逆运动学是指已知机身相对于世界坐标系的运动速度指令(包括了三个方向的移动速度和三个方向的转动速度)后得出支撑足端相对于机身的速度，再通过雅克比矩阵求逆得出关节角速度，积分可得关节角度，从而计算执行器所需的位移量，驱动执行器，完成支撑相步态。具体算法如下：
Figure BDA0003644438830000123
Figure BDA0003644438830000124
足端在世界坐标系下的移动速度，
Figure BDA0003644438830000125
足端在机身坐标系下的移动速度，
Figure BDA0003644438830000126
机身坐标系相对于世界坐标系的旋转变换矩阵，
Figure BDA0003644438830000127
机身坐标系相对于世界坐标系的旋转速度变换矩阵，
Figure BDA0003644438830000128
机身坐标系相对于世界坐标系的移动速度指令，
其中，
Figure BDA0003644438830000131
Rot(z,α)表示绕世界坐标系z轴旋转α角，Rot(y,β)表示绕世界坐标系y轴旋转β角，Rot(x,γ)表示绕世界坐标系x轴旋转γ角；
α：偏航角(Yaw)，β：俯仰角(Pitch)，γ：横滚角(Roll)。
Figure BDA0003644438830000132
Ji：i号腿的速度雅克比矩阵，
Figure BDA0003644438830000133
i号腿的各个关节角速度，L0：根关节与髋关节之间的连杆长度，L1：髋关节与膝关节之间的连杆长度，L2：膝关节与足端之间的连杆长度，θi0：i号腿的根关节旋转角度，θi1：i号腿的髋关节旋转角度，θi2：i号腿的膝关节旋转角度
Figure BDA0003644438830000134
Figure BDA0003644438830000135
Figure BDA0003644438830000136
ωx、ωy、ωz分别是机身坐标系相对于世界坐标系的XW、YW、ZW三轴的旋转角速度。
Figure BDA0003644438830000137
即为期望姿态角与IMU反馈的实际姿态角之间的偏差信号经过PID运算后得到的姿态角速度指令
Figure BDA0003644438830000138
Figure BDA0003644438830000139
Figure BDA00036444388300001310
Figure BDA00036444388300001311
θi：i号腿计算执行器位移所需的三个关节角度值，θi_initial：单条腿在由摆动到触地成为支撑腿的时刻的三个关节角度的初始值。t：该腿在进入支撑相到结束支撑相的整个支撑时长。
5.步态判别方法：机器人只有在支撑相与摆动相不断交替的过程中才能实现行走，对于单条腿来说，不是处于摆动相就是处于支撑相，单腿进入支撑相(由摆动轨迹规划转为支撑轨迹规划)的条件是：摆动过程中检测到由足端力传感器反馈的触地信号；单腿进入摆动相(由支撑轨迹规划转为摆动轨迹规划)的条件是：另一组主支撑腿由摆动相全部着地进入支撑相。
6.步态切换方法：虽然机器人的单腿在摆动相与支撑相时采用不同的关节轨迹生成方法，但要保证摆动相的起点是支撑相的终点，摆动相的终点是支撑相的起点，这样摆动相与支撑相才能组成连续完整的足端轨迹。步态切换时的规划方式如下：在摆动相切换至支撑相时，将切换时刻的关节角度作为关节角速度积分的初始值，在支撑相切换至摆动相时，将切换时刻的足端位置作为足端轨迹的起点，再结合第5点中的步态判别方法就可以实现机器人连续不断地行走。
7.控制框图的整个坡地行走和姿态控制过程可以描述为：
(1)人为给出行走速度和行走方向指令后，分析1、4、5号腿所构成的主支撑面信息，规划机身坐标系相对于世界坐标系的移动速度指令
Figure BDA0003644438830000141
和姿态角指令Ee，标识符LR置0，1、4、5号腿的关节角度使用支撑相的步态规划，2、3、6号腿的关节角度使用摆动相的步态规划。
(2)将Ee经过PID运算后得到的值作为姿态角速度指令
Figure BDA0003644438830000142
将
Figure BDA0003644438830000143
和
Figure BDA0003644438830000144
共同输入速度逆运动学进行计算，得到关节角速度指令
Figure BDA0003644438830000145
经过积分得到关节角度指令θi(i＝1,4,5)，作为1、4、5号腿的关节驱动量。将2、3、6号腿的足端初始位置作为六次插值的起始位置，计算摆动腿足端轨迹，经过腿部几何逆解，得到关节角度指令θi(i＝2,3,6)作为2、3、6号腿的关节驱动量。1、4、5号腿开始支撑，2、3、6号腿开始摆动。
(3)2、3、6号腿在摆动的过程中，陆续触地，在触地的瞬间进入支撑相步态规划，将触地瞬间的关节角速度作为积分初值，通过速度逆运动学方式得出关节角速度，积分得到关节驱动量，与原先支撑的1、4、5号腿一起，共同支撑机身。
(4)在2、3、6号腿都触地之后，分析2、3、6号腿所构成的主支撑面信息，重新规划机身坐标系相对于世界坐标系的移动速度指令
Figure BDA0003644438830000151
和姿态角指令Ee，标识符LR置1，2、3、6号腿开始继续支撑，1、4、5号腿开始摆动，规划方法同(2)。
(5)1、4、5号腿在摆动的过程中，陆续触地，在触地的瞬间进入支撑相步态规划，规划方法同(3)。
(6)在1、4、5号腿都触地之后，回到上述步骤(1)，完成一个步态周期，不断重复(1)至(5)的过程，步态就不断循环，机器人就可以腿的不断支撑与摆动交替的过程中实现坡地崎岖地形上的连续行走。
图4是使用本发明的控制策略进行仿真情况，使用Simulink/Adams联合仿真的方式，控制程序在Simulink中完成，机械部分在Adams中完成。图4的第一幅(左图)是机器人在水平地面上直行的仿真情况，前进速度0.375m/s，行走过程中机身保持水平，机身高度为设定值1.020m。图4的第二幅(中间)是机器人在小坡崎岖地面上的直行仿真情况，该地形是用样条差值曲线绘制的一个随机地形，前进速度0.375m/s，行走过程中机身保持水平，机身高度为设定值1.020m。图4的第三幅(右图)是机器人从水平地面爬上15°斜坡的仿真情况，该图中机器人位于地形的交界处，在交界处不断调整自身俯仰角，使机身在坡面上时能够与坡面保持平行，前进速度0.2m/s，机身高度为设定值1.020m。从仿真结果来看，使用该算法，机器人能够顺利通过这些地形，且姿态角度控制效果理想。

Claims (10)

1.一种步行机器人坡地崎岖地形的步态生成与姿态控制方法，其特征在于，所述步行机器人包括机身和至少6条腿，将机身一周的所有腿中相互间隔的腿分为同一组，共可分为两组，两组腿各称为一组主支撑腿，每条腿有支撑相和摆动相两种状态；同一组的各条腿均处于支撑相时，该组各条腿的足端所构成的平面为主支撑面，
定义机身坐标系{XB,YB,ZB}、世界坐标系{XW,YW,ZW}和主支撑面坐标系{XS,YS,ZS}，机身坐标系原点OB位于机身形心位置，过机身坐标系原点作主支撑面的垂线，垂足为主支撑面坐标系原点OS，ZS方向为主支撑面法向，XS的方向与XW沿重力方向在主支撑面上的投影轴方向一致，将OB到主支撑面的垂直距离定义为机身高度H，
所述步态生成与姿态控制方法包括以下步骤：
(1)人为给出行走速度和行走方向指令后，将处于支撑相的一组腿称为第一组腿，另一组腿称为第二组腿，分析处于支撑相的第一组腿所构成的主支撑面信息，规划机身坐标系相对于世界坐标系的移动速度指令
Figure FDA0003644438820000011
和姿态角指令Ee，用来区分哪一组腿支撑的标识符LR置0，处于支撑相的第一组腿的关节角度使用支撑相的步态规划，第二组腿的关节角度使用摆动相的步态规划；
(2)将Ee经过PID运算后得到的值作为姿态角速度指令
Figure FDA0003644438820000012
将
Figure FDA0003644438820000013
和
Figure FDA0003644438820000014
共同输入速度逆运动学进行计算，得到关节角速度指令
Figure FDA0003644438820000015
经过积分得到关节角度指令θi，i为腿的编号，作为处于支撑相的第一组腿的关节驱动量；将第二组腿的足端初始位置作为摆动轨迹的起始位置，采用六次插值方式规划摆动腿足端轨迹，经过腿部几何逆解，得到关节角度指令θi，i为腿的编号，作为处于摆动的第二组腿的关节驱动量；
(3)第二组腿在摆动的过程中，陆续触地，在触地的瞬间进入支撑相步态规划，将触地瞬间的关节角速度作为积分初值，通过速度逆运动学方式得出关节角速度，积分得到关节驱动量，与原先支撑的第一组腿一起，共同支撑机身。
(4)在第二组腿都触地之后，分析第二组腿所构成的主支撑面信息，重新规划机身坐标系相对于世界坐标系的移动速度指令
Figure FDA0003644438820000016
和姿态角指令Ee，标识符LR置1，第二组腿继续支撑，第一组腿开始摆动，规划方法同步骤(2)；
(5)第一组腿在摆动的过程中，陆续触地，在触地的瞬间进入支撑相步态规划，规划方法同步骤(3)。
(6)在第一组腿都触地之后，回到步骤(1)，完成一个步态周期，重复步骤(1)至(5)的过程，步态进行循环，步行机器人在腿的不断支撑与摆动交替的过程中实现坡地崎岖地形上的连续行走。
2.根据权利要求1所述的步态生成与姿态控制方法，其特征在于，步骤(1)中，当第一组腿触地后，确定主支撑面坐标系，该主支撑面坐标系在世界坐标系下的姿态可以表示为：
Figure FDA0003644438820000021
其中，
Figure FDA0003644438820000022
均是3×1的列向量，
Figure FDA0003644438820000023
表示XS的单位向量在世界坐标系的三个轴上的投影值，
Figure FDA0003644438820000024
表示YS的单位向量在世界坐标系的三个轴上的投影值，
Figure FDA0003644438820000025
表示ZS的单位向量在世界坐标系的三个轴上的投影值；
Figure FDA0003644438820000026
的计算方法如下：
Figure FDA0003644438820000027
其中，WPi，i＝1～6，表示各条腿的足端在世界坐标系下的坐标位置，通过机器人正运动学和经典的坐标变换可以得到，上标“→”表示是由足端点组成的向量，“|| ||”表示取向量模长，
由机器人正运动学可以得到的各条腿足端在机身坐标系下的坐标位置BPi，由处于支撑相的一组腿的3个足端坐标可以计算得到主支撑面在机身坐标系下的平面方程，并由机身坐标系原点到平面的距离公式确定机身高度H。
3.根据权利要求2所述的步态生成与姿态控制方法，其特征在于，步骤(1)和(4)中，第一组腿和第二组腿在相互切换支撑和摆动的过程中，由于主支撑面的改变，都会重新产生一个目标姿态角，相邻的两个目标姿态角之间需要通过插值的方式规划出一条轨迹值，作为姿态角指令Ee。
4.根据权利要求3所述的步态生成与姿态控制方法，其特征在于，期望机身与主支撑面平行时，需要使ZB与ZS平行，ZS的单位向量在世界坐标系中的表达式为
Figure FDA0003644438820000031
中的
Figure FDA0003644438820000032
解算出机身姿态角的目标值即为目标姿态角，具体算法如下：
Figure FDA0003644438820000033
Figure FDA0003644438820000034
表示机身坐标系相对于世界坐标系的目标旋转矩阵，Rot(z,αe)表示绕世界坐标系z轴旋转αe角，Rot(y,βe)表示绕世界坐标系y轴旋转βe角，Rot(x,γe)表示绕世界坐标系x轴旋转γe角，
αe：目标偏航角，βe：目标俯仰角，γe：目标横滚角，s和c分别为三角函数sin和cos的简写，
令
Figure FDA0003644438820000035
直行时αe＝0，因此，
Figure FDA0003644438820000036
可以列出两个独立方程，βe,γe∈[-90°,90°]范围内，求出βe、γe角的唯一解，
Figure FDA0003644438820000037
Figure FDA0003644438820000038
分别表示
Figure FDA0003644438820000039
向量的第1、2、3个值。
5.根据权利要求2所述的步态生成与姿态控制方法，其特征在于，步骤(1)中，移动速度指令
Figure FDA00036444388200000310
通过结合人为输入的行进速度指令
Figure FDA00036444388200000311
以及机身高度H得到，人为输入的行进速度是相对于主支撑面来说的，需要向世界坐标系投影，期望机身沿XS方向有一个速度
Figure FDA0003644438820000041
则该速度在世界坐标系的投影速度为
Figure FDA0003644438820000042
期望机身高度是一个定值He，则在主支撑面切换的时刻要对高度进行补偿，给出以一个沿ZS方向的速度
Figure FDA0003644438820000043
则该速度在世界坐标系的投影速度为
Figure FDA0003644438820000044
最终移动速度指令
Figure FDA0003644438820000045
6.根据权利要求1所述的步态生成与姿态控制方法，其特征在于，步骤(2)中，在机器人行走过程中姿态角的实时控制时，引入机身上陀螺仪IMU的实时数据作为反馈，与规划的姿态角轨迹Ee作差，采用PID控制策略，将偏差值作为姿态角速度指令
Figure FDA0003644438820000046
引入机器人速度逆运动学模块进行计算。
7.根据权利要求6所述的步态生成与姿态控制方法，其特征在于，机器人速度逆运动学模块中的机器人速度逆运动学是指已知机身相对于世界坐标系的运动速度指令，包括三个方向的移动速度
Figure FDA0003644438820000047
和三个方向的转动速度
Figure FDA0003644438820000048
然后得出支撑足端相对于机身的速度，再通过雅克比矩阵求逆得出关节角速度，积分可得关节角度θi，从而计算执行器所需的位移量，驱动执行器，完成支撑相步态。
8.根据权利要求7所述的步态生成与姿态控制方法，其特征在于，关节角度指令θi的具体求解算法如下：
Figure FDA0003644438820000049
Figure FDA00036444388200000410
足端在世界坐标系下的移动速度，
Figure FDA00036444388200000411
足端在机身坐标系下的移动速度，
Figure FDA00036444388200000412
机身坐标系相对于世界坐标系的旋转变换矩阵，
Figure FDA00036444388200000413
机身坐标系相对于世界坐标系的旋转速度变换矩阵，
Figure FDA00036444388200000414
机身坐标系相对于世界坐标系的移动速度指令，
其中，
Figure FDA0003644438820000051
Rot(z,α)表示绕世界坐标系z轴旋转α角，Rot(y,β)表示绕世界坐标系y轴旋转β角，Rot(x,γ)表示绕世界坐标系x轴旋转γ角，
α：偏航角，β：俯仰角，γ：横滚角，s和c分别为三角函数sin和cos的简写，
Figure FDA0003644438820000052
Ji：i号腿的速度雅克比矩阵，
Figure FDA0003644438820000053
i号腿的各个关节角速度，L0：根关节与髋关节之间的连杆长度，L1：髋关节与膝关节之间的连杆长度，L2：膝关节与足端之间的连杆长度，θi0：i号腿的根关节旋转角度，θi1：i号腿的髋关节旋转角度，θi2：i号腿的膝关节旋转角度，
Figure FDA0003644438820000054
Figure FDA0003644438820000055
Figure FDA0003644438820000056
ωx、ωy、ωz分别是机身坐标系相对于世界坐标系的XW、YW、ZW三轴的旋转角速度，
Figure FDA0003644438820000057
即为期望姿态角与IMU反馈的实际姿态角之间的偏差信号经过PID运算后得到的姿态角速度指令
Figure FDA0003644438820000058
Figure FDA0003644438820000059
Figure FDA00036444388200000510
Figure FDA00036444388200000511
θi：i号腿计算执行器位移所需的三个关节角度值，θi_initial：单条腿在由摆动到触地成为支撑腿的时刻的三个关节角度的初始值，t：该腿在进入支撑相到结束支撑相的整个支撑时长。
9.根据权利要求1所述的步态生成与姿态控制方法，其特征在于，单腿进入支撑相的条件是：摆动过程中检测到由足端力传感器反馈的触地信号；单腿进入摆动相的条件是：另一组主支撑腿由摆动相全部着地进入支撑相。
10.根据权利要求9所述的步态生成与姿态控制方法，其特征在于，单腿步态切换时的规划方式如下：在摆动相切换至支撑相时，将切换时刻的关节角度作为关节角速度积分的初始值，在支撑相切换至摆动相时，将切换时刻的足端位置作为足端轨迹的起点。
CN202210526150.0A 2022-05-16 2022-05-16 一种步行机器人坡地崎岖地形的步态生成与姿态控制方法 Active CN114995476B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210526150.0A CN114995476B (zh) 2022-05-16 2022-05-16 一种步行机器人坡地崎岖地形的步态生成与姿态控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210526150.0A CN114995476B (zh) 2022-05-16 2022-05-16 一种步行机器人坡地崎岖地形的步态生成与姿态控制方法

Publications (2)

Publication Number Publication Date
CN114995476A true CN114995476A (zh) 2022-09-02
CN114995476B CN114995476B (zh) 2022-11-29

Family

ID=83027977

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210526150.0A Active CN114995476B (zh) 2022-05-16 2022-05-16 一种步行机器人坡地崎岖地形的步态生成与姿态控制方法

Country Status (1)

Country Link
CN (1) CN114995476B (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 哈尔滨工业大学 一种电驱动六足机器人运动控制系统及方法
CN107065867A (zh) * 2017-03-28 2017-08-18 浙江大学 一种面向未知崎岖地形的四足机器人运动规划方法
CN111309039A (zh) * 2020-02-27 2020-06-19 杭州云深处科技有限公司 一种四足机器人姿态控制方法和装置
WO2022027914A1 (zh) * 2020-08-07 2022-02-10 深圳市优必选科技股份有限公司 仿人机器人步态规划方法、装置和仿人机器人

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 哈尔滨工业大学 一种电驱动六足机器人运动控制系统及方法
CN107065867A (zh) * 2017-03-28 2017-08-18 浙江大学 一种面向未知崎岖地形的四足机器人运动规划方法
CN111309039A (zh) * 2020-02-27 2020-06-19 杭州云深处科技有限公司 一种四足机器人姿态控制方法和装置
WO2022027914A1 (zh) * 2020-08-07 2022-02-10 深圳市优必选科技股份有限公司 仿人机器人步态规划方法、装置和仿人机器人

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
GANG CHEN等: "position-posture trajectory tracking of a six-legged walking robot", 《INTERNATIONAL JOURNAL OF ROBOTICS AND AUTOMATION》 *
任杰等: "双足机器人的两种步态规划的解耦分析及比较", 《软件》 *
常青等: "六足机器人不规则地形下的运动规划算法研究", 《计算机测量与控制》 *
李辉: "基于质心动量动力学的六轮腿移动机器人"龙骧"的运动规划与控制策略", 《中国博士学位论文全文数据库 信息科技辑》 *
杨雪锋: "基于CPG反馈模型的六足机器人斜坡步态规划", 《中国硕士学位论文全文数据库》 *
罗斌: "基于落地时间的四足机器人对角运动平衡控制方法", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *

Also Published As

Publication number Publication date
CN114995476B (zh) 2022-11-29

Similar Documents

Publication Publication Date Title
Chen et al. Flexible gait transition for six wheel-legged robot with unstructured terrains
CN111497965B (zh) 一种轮足切换机器人系统及其控制方法
CN111070201B (zh) 四足机器人在负载突变下基于zmp理论的反应式鲁棒控制方法
WO2017092463A1 (zh) 一种双足机器人的步态控制方法和装置
JP3599244B2 (ja) ロボット装置、ロボット装置の運動制御装置並びに運動制御方法
US7053577B2 (en) Robot and motion control method of robot
CN105137969B (zh) 基于支撑线运动分解的四足机器人trot步态本体位姿控制方法
US8682488B2 (en) Humanoid robot and walking control method thereof
CN105666498B (zh) 提高六足机器人行走稳定性的步态规划方法
CN101068663B (zh) 腿式移动机器人及其控制方法
Gong et al. Bionic quadruped robot dynamic gait control strategy based on twenty degrees of freedom
CN104898672B (zh) 一种仿人机器人行走轨迹的优化控制方法
CN107045552A (zh) 一种基于正弦对角步态与快速查表法的四足机器人运动控制方法及控制装置
CN113835429B (zh) 一种仿生双足机器人的控制方法及仿生双足机器人
Vladareanu et al. Modeling and hybrid position-force control of walking modular robots
CN113156836A (zh) 一种基于Simscape Multibody的多足机器人虚拟样机运动控制仿真系统
CN115793683A (zh) 一种重心控制方法、调整机构及足式机器人
CN116661293B (zh) 一种轮腿式车辆跳跃障碍的运动控制方法及系统
CN114454983B (zh) 一种四足机器人转弯控制方法及系统
CN113843799B (zh) 一种四足机器人姿态复位控制方法、装置及存储介质
CN112744306A (zh) 一种基于运动学的四足爬壁机器人转身步态控制方法
Han et al. A heuristic gait template planning and dynamic motion control for biped robots
CN114995476B (zh) 一种步行机器人坡地崎岖地形的步态生成与姿态控制方法
Ma et al. Trotting gait control of quadruped robot based on Trajectory Planning
Xiao et al. Improvement of winding gait for snake robot

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