技术领域
[0001] 本
发明涉及一种运动控制框架，特别是一种多足支撑步行机器人的运动控制框架，具体来说是一种基于并联机构思维的多足支撑步行机器人运动控制框架。
背景技术
[0002] 在复杂地形下，如何使步行机器人平稳地向期望的目标行走是其主要难点。这要求对机器人进行运动规划、轨迹规划和
位姿控制等，运动规划用于规划机器人的运动路径，轨迹规划使得机器人在特定路径上按一定的运动学规律运动，位姿控制用于降低机器人移动过程中的
位置和
姿态误差以保证机器人的平稳运行。
[0003] 合理的运动控制框架设计对于步行机器人的运动是非常重要的，当前多足支撑步行机器人(如四足机器人、
六足机器人)对
机身的控制大都基于
串联机构思维：把机身当作
基座，而把各腿当作独立的串联支链，将机身的运动看作各腿相对于机身的相对摆动，进而通过规划和控制腿部运动来达到控制机身的目的。基于这种思想，足式机器人的运动需要按一定顺序依次规划各腿足端轨迹，并且为了控制步行机器人的运动方向，需要分别按特定方向规划足端轨迹，再进行轨迹融合；而为了控制步行机器人的运动姿态，需要将部分机身的位姿控制融入轨迹规划中，以使步行机器人按一定的位姿要求沿设定方向运动。这种基于串联机构思维进行的机器人运动控制主要通过规划和控制腿部运动来达到控制机身运动的目的，因此对于机身的运动控制不够直观；由于腿部规划和机器人机身位姿控制相互耦合，使得轨迹规划复杂化，增大了机器人位姿控制难度与调试复杂度，也不利于规划和位姿控制
算法的合理性分析和功能的优化。
发明内容
[0004] 为了避免上述基于串联机构思维进行足式机器人运动控制造成的问题，本发明提出一种基于并联机构思维的多足支撑步行机器人运动控制框架，基于并联机构思维的运动控制框架使得步行机器人的运动规划问题和位姿控制问题可直接针对机器人的机身来进行设计，大大简化机器人机身的规划和位姿控制。
[0005] 本发明基于并联机构思维的多足支撑步行机器人运动控制框架，将多足支撑的步行机器人视为并联机构来对其运动进行规划和控制。首先，对于支撑腿控制来说，将步行机器人的机身作为并联机构动平台，将步行机器人多个支撑足与地面
接触部分作为并联机构静平台，将步行机器人各支撑腿作为并联机构不同的支链。由此，步行机器人机身运动即可视为并联机构动平台的运动。其中机器人的前进、后退、上下运动即可视为动平台的平动；机器人机身的倾斜、转向运动可视为动平台的转动；最终通过动平台的位姿规划和位姿控制来使步行机器人按期望的全方位运动目标运动。对于步行机器人运动过程中出现的机身位姿偏差，通过动平台的位姿控制降低，且最终将位姿控制归结到关节控制中，将机身的移动、倾斜、转向问题作为位姿规划与位姿控制问题。
[0006] 本发明基于并联机构思维的多足支撑步行机器人运动控制框架，主要基于三个
坐标系，分别为:
[0007] A、世界坐标系或者导航坐标系W(Xw，Yw，Zw)，表示机器人的世界坐标参考系；
[0008] B、机器人机身平面坐标系B(XB，YB，ZB)，该坐标系建立在动平台上，原点一般为机器人机身的几何中心，表示并联机构动平台固连的坐标参考系；
[0009] C、机器人支撑腿足端与地面接触点构成的平面所在的坐标系S(Xs，Ys，Zs)，表示并联机构静平台所在的坐标参考系；
[0010] 本发明基于并联机构思维的多足支撑步行机器人运动控制框架，具体流程为：
[0011] (1)对步行机器人运动控制所需要的参数、机器人状态与坐标系定义等进行初始化设置。
[0012] (2)确定机器人的支撑腿和摆动腿。
[0013] (3)根据支撑状态更新坐标系S，得到S坐标系相对于W坐标系的位置和姿态。
[0014] (4)坐标系W下机身质心位姿规划。
[0015] 在坐标系W下，根据初始化参数对坐标系B进行运动规划，确定坐标系W下的坐标系B位姿规划值。
[0016] (5)将坐标系W下的坐标系B位姿规划值转换为坐标系S下的规划值。
[0017] (6)根据坐标系S下机身质心位姿规划点坐标，确定坐标系S下的
机器人关节角度值或者关节约束关系。
[0018] (7)根据坐标系S下的机器人
关节角度值或者关节约束关系，确定驱动关节的期望角度值。
[0019] (8)根据驱动关节的期望角度值和由
传感器测量的实际关节位置或速度进行关节位置控制。
[0020] (9)获取坐标系S下关节角度测量值和坐标系W下机身姿态测量值。
[0021] (10)根据S坐标系下的关节角度测量值，得到B坐标系相对于S坐标系的位姿。
[0022] (11)根据S坐标系下的B坐标系位姿测量值、S坐标系相对于W坐标系的位姿和B坐标系相对于W坐标系的姿态，得到B坐标系相对于W坐标系的位姿。
[0023] (12)根据W坐标系下的机身位姿规划值和测量值进行机身位姿控制，将位姿误差转化为B坐标系相对于W坐标系的六维虚拟
力/力矩。
[0024] (13)将B坐标系相对于W坐标系的六维虚拟力/力矩，利用S坐标系相对于W坐标系的位姿转化为B坐标系相对于S坐标系的六维虚拟力/力矩。
[0025] (14)根据B坐标系相对于S坐标系的六维虚拟力/力矩，将该六维虚拟力/力矩转化为S坐标系的关节力矩期望值或者关节力矩约束关系。
[0026] (15)驱动关节力矩。
[0027] 根据S坐标系的关节力矩期望值或者关节力矩的约束关系，确定驱动关节力矩期望值。
[0028] (16)关节位置与力矩混合控制。
[0029] 根据驱动关节力位置期望值和测量值、力矩期望值和反馈值进行关节混合控制，控制支撑腿关节运动。
[0030] (17)W系确定摆动腿落足点期望位置。
[0031] 在步骤2中确定摆动腿后，根据机器人状态和机器人机身运动期望值，确定摆动腿在摆动结束后与地面接触的期望位置。
[0032] (18)确定摆动腿在摆动结束后与地面接触的期望位置后，基于W系进行摆动腿足端轨迹规划，确定摆动腿足端从开始摆动至落足点之间的位置期望值。
[0033] (19)根据W坐标系下的摆动腿足端位置期望值和B坐标系相对于W坐标系的位姿，确定摆动腿相对于B坐标系下的足端位置期望值。
[0034] (20)根据足端相对于B坐标系的位置期望值，确定摆动腿驱动关节角度期望值。
[0035] (21)根据摆动腿驱动关节期望值和测量值进行关节位置控制，控制摆动腿驱动关节运动。
[0036] (22)根据摆动腿关节角度测量值，得到摆动腿足端相对于B坐标系的位置测量值。
[0037] (23)根据B坐标系相对于W坐标系的位姿与摆动腿足端相对于B系的位置，确定摆动腿足端相对于W坐标系的位置。
[0038] (24)检测摆动腿足端是否与地面接触。
[0039] (25)若摆动腿与地面无接触，则转到步骤(24)。若摆动腿与地面接触，则转到下一步。
[0040] (26)根据运动规划的摆腿序列或支撑腿与摆动腿切换
信号切换所有的摆动腿与支撑腿，返回步骤2，确定新的支撑腿与摆动腿，继续进行步骤3～26。
[0041] 本发明优点在于：
[0042] 1、本发明基于并联机构思维的多足支撑步行机器人运动控制框架，合理、清晰而具体地阐述了多足支撑步行机器人的运动控制流程，提供了一套非常完整地多足支撑步行机器人开发的技术路线，对于该类机器人的技术开发具有重要的指导意义。
[0043] 2、本发明基于并联机构思维的多足支撑步行机器人运动控制框架，将机身的运动和控制转化为并联机构动平台的位姿规划和位姿控制，而将腿部规划和落足点控制用于更新并联机构的动平台位姿，实现了机器人规划和控制的解耦，大大降低了规划和控制的复杂度，也便于规划和位姿控制的功能分析与优化。
附图说明
[0044] 图1为本发明基于并联机构思维的多足支撑步行机器人运动控制框架所基于的坐标系示意图；
[0045] 图2为本发明基于并联机构思维的多足支撑步行机器人运动控制框架
流程图。
具体实施方式
[0046] 下面结合附图对本发明作进一步详细说明。
[0047] 本发明基于并联机构思维的多足支撑步行机器人运动控制框架，基于并联机构思维，将多足支撑的步行机器人视为并联机构来对其运动进行规划和控制。
[0048] 首先，对于支撑腿控制来说，将步行机器人的机身作为并联机构动平台，将步行机器人多个支撑足与地面接触部分作为并联机构静平台，将步行机器人各支撑腿作为并联机构不同的支链。由此，步行机器人机身运动即可视为并联机构动平台的运动；其中机器人的前进、后退、上下运动即可视为动平台的平动；机器人机身的倾斜、转向运动可视为动平台的转动。这样便可以通过动平台的位姿规划来使步行机器人按期望的全方位运动目标运动，从而将步行机器人的全方位移动问题只作为动平台的位置规划与位置控制问题。而对于步行机器人运动过程中出现的机身位姿(位置和姿态)偏差，则通过动平台的位姿控制来尽可能降低，并且最终将姿态控制归结到关节控制中，如此将机身的倾斜、转向问题只作为姿态规划与姿态控制问题。
[0049] 因此本发明将步行机器人的运动完全拆分为两部分，分别为步行机器人全方位运动的规划部分和步行机器人机身的位姿控制部分，实现功能解耦。对于摆动腿部分对于步行机器人摆动腿的轨迹规划和控制，则以动平台为
基础，基于串联支链进行轨迹规划和控制，因而摆动腿的规划和控制本身就是解耦的。当摆动腿触地后，则根据触地检测将当前支撑腿切换为摆动腿，而将当前摆动腿切换为支撑腿，并重新按照基于并联机构思维的支撑腿规划与控制和基于串联机构思维的摆动腿规划和控制进行后续操作。
[0050] 本发明基于并联机构思维的多足支撑步行机器人运动控制框架，主要基于三个坐标系，如图1所示，分别为:
[0051] A、世界坐标系或者导航坐标系W(Xw，Yw，Zw)，表示机器人的世界坐标参考系；
[0052] B、机器人机身平面(相当于并联机构动平台)坐标系B(XB，YB，ZB)，该坐标系建立在动平台上，原点一般为机器人机身的几何中心，表示并联机构动平台固连的坐标参考系；
[0053] C、机器人支撑腿足端与地面接触点构成的平面(相当于并联机构静平台)所在的坐标系S(Xs，Ys，Zs)，表示并联机构静平台所在的坐标参考系。
[0054] 基于上述三个坐标系，本发明基于并联机构思维的多足支撑步行机器人运动控制框架基本流程如下：
[0055] (1)对步行机器人运动控制所需要的参数、机器人状态与坐标系定义等进行初始化设置。如：确定初始状态下机器人关节位置、关节速度、关节力矩、足底力、支撑状态等反馈量；确定初始机器人状态量如机身初始速度、初始姿态、步态规划中的腿部运动方式和顺序；确定初始状态下世界坐标系W、机器人支撑平面所在坐标系S、机身坐标系的原点位置和坐标轴方向；确定三个坐标系之间的初始变换关系。
[0056] (2)根据机器人初始化状态或者根据触地检测确定支撑腿和摆动腿。
[0057] (3)确定支撑腿后，可根据支撑状态更新坐标系S，得到S坐标系相对于W坐标系的位置和姿态。
[0058] (4)坐标系W下机身质心位姿规划。
[0059] 在坐标系W下，根据初始化参数对坐标系B进行运动规划，即在坐标系W下对机身质心坐标和机身姿态进行运动规划，确定坐标系B原点位置和姿态(机身姿态)在坐标系W下的一系列目标值，即坐标系W下的坐标系B位姿规划值。
[0060] (5)坐标系S下机身质心位姿规划。
[0061] 将坐标系W下的坐标系B位姿规划值转换为坐标系S下的规划值，即将动平台相对于坐标系W的位姿轨迹规划目标点转换为动平台相对于坐标系S下的位姿轨迹规划目标点。
[0062] (6)坐标系S下并联机构逆运动学。
[0063] 根据坐标系S下机身质心位姿规划点坐标，利用并联机构逆运动学确定坐标系S下的机器人关节角度值或者关节约束关系。
[0064] (7)根据坐标系S下的机器人关节角度值或者关节约束关系，确定驱动关节的期望角度值。
[0065] (8)关节位置与力矩混合控制。
[0066] 根据驱动关节的期望角度值和由传感器测量的实际关节位置或速度进行关节位置控制，该部分是关节混合控制的一部分。
[0067] (9)获取坐标系S下关节角度测量值和坐标系W下机身姿态测量值。
[0068] 根据传感器测量驱动关节角度测量值，并转换到S坐标系下的关节角度测量值；利用IMU(惯性测量元件)或状态估计得到W坐标系下的机身姿态测量值，即B坐标系相对于W坐标系的姿态。
[0069] (10)S系并联机构正运动学。
[0070] 根据S坐标系下的关节角度测量值，利用并联机构正运动学，得到B坐标系相对于S坐标系的位姿。
[0071] (11)W系机身位姿测量值。
[0072] 根据S坐标系下的B坐标系位姿测量值、S坐标系相对于W坐标系的位姿和B坐标系相对于W坐标系的姿态，利用坐标变换和信息融合得到B坐标系相对于W坐标系的位姿，即机身相对于世界坐标系的位姿。
[0073] (12)W系机身位姿虚拟力控制得到W坐标系机身虚拟六维力/力矩。
[0074] 根据W坐标系下的机身位姿规划值和测量值进行机身位姿控制(如虚拟力控制)，将位姿误差转化为B坐标系相对于W坐标系的六维虚拟力/力矩。
[0075] (13)S系机身六维力/力矩。
[0076] 将根据机身位姿控制得到的B坐标系相对于W坐标系的六维虚拟力/力矩，利用S坐标系相对于W坐标系的位姿转化为B坐标系相对于S坐标系的六维虚拟力/力矩，即并联机构动平台相对于静平台的六维虚拟力/力矩。
[0077] (14)S系并联机构逆动力学。
[0078] 根据B坐标系相对于S坐标系的六维虚拟力，利用并联机构逆动力学或者静力学平衡方程，将该六维虚拟力/力矩转化为S坐标系的关节力矩期望值或者关节力矩约束关系[0079] (15)驱动关节力矩。
[0080] 根据S坐标系的关节力矩期望值或者关节力矩的约束关系确定驱动关节力矩期望值。
[0081] (16)关节位置与力矩混合控制。
[0082] 根据驱动关节力矩期望值和反馈值(关节力矩测量值或关节力矩算法估计值)进行力矩控制，该部分是关节混合控制的一部分，它与步骤(8)的关节位置控制结合形成关节混合控制，控制支撑腿关节运动。
[0083] (17)W系确定摆动腿落足点期望位置。
[0084] 在步骤2中确定支撑腿后，可根据机器人状态和机器人机身运动期望值，确定摆动腿在摆动结束后与地面接触的期望位置(落足点)，如通过
弹簧倒立摆模型确定摆动腿在摆动结束后与地面接触的期望位置，即摆动腿落足点。
[0085] (18)W系摆动腿足端轨迹规划。
[0086] 确定摆动腿落足点之后，可基于W系进行摆动腿足端轨迹规划，确定摆动腿足端从开始摆动至落足点之间的一系列位置期望值。
[0087] (19)B系摆动腿足端轨迹。
[0088] 根据W坐标系下的摆动腿足端位置期望值和B坐标系相对于W坐标系的姿态，确定摆动腿相对于B坐标系下的足端位置期望值。
[0089] (20)B系单腿逆运动学。
[0090] 根据足端相对于B坐标系的位置期望值，利用摆动腿足端相对于B坐标系的单腿逆运动学和足端相对于B坐标系的位置期望值，确定摆动腿驱动关节角度期望值。
[0091] (21)摆动腿关节控制。
[0092] 根据摆动腿驱动关节期望值和测量值进行关节位置控制，控制摆动腿驱动关节运动。
[0093] (22)B系摆动腿足端位置测量值。
[0094] 根据摆动腿关节角度测量值，利用摆动腿足端相对于B坐标系的单腿正运动学，得到摆动腿足端相对于B坐标系的位置测量值。
[0095] (23)W系摆动腿足端位置。
[0096] 根据B坐标系相对于W坐标系的位姿与摆动腿足端相对于B系的位置，确定摆动腿足端相对于W坐标系的位置。
[0097] (24)触地检测。根据摆动腿足端位置、
电流或足端触地传感器，检测摆动腿足端是否与地面接触。
[0098] (25)若触地检测状态为否，即没有检测到摆动腿与地面接触，则转(24)步；若触地检测状态为是，即检测到摆动腿与地面接触，则转下一步。
[0099] (26)切换支撑腿与摆动腿。
[0100] 触地检测状态为是后，根据运动规划的摆腿序列或支撑腿与摆动腿切换信号切换所有的摆动腿与支撑腿，返回步骤2，确定新的支撑腿与摆动腿；确定新的支撑腿后转第(3)步骤继续执行，确定新的摆动腿后转第(17)步骤继续执行。
[0101] 上述步骤(3)至(16)为支撑腿部分，(17)至(25)步骤为摆动腿部分，两部分是同步进行的。
[0102] 本发明基于并联机构思维的多足支撑步行机器人运动控制框架中，某一个环节的功能实现具有很大的灵活性，对于某一具体环节没有严格限制实现该环节的具体方法，而是考虑了实现某一个环节功能可能存在的多种实现方案，在具体方法上仅出参考实现方案，因此本发明所述运动控制框架既保证了控制框架的技术指导意义，也兼顾了给予开发人员足够的实现方案创造空间。
