CN110497414B - 四足机器人连续躯干运动轨迹规划方法 - Google Patents

Abstract

一种四足机器人连续躯干运动轨迹规划方法，包括以下步骤：（1）将四足机器人在躯干摆动阶段各支撑足投影至平面坐标系{ ^P O}中；（2）根据各支撑足在平面坐标系{ ^P O}中的投影位置及设定的能够改变躯干侧移量的可调整参数，确定躯干运动轨迹的关键路径点；（3）根据确定的躯干运动轨迹的关键路径点以及躯干运动的边界条件，给出躯干运动轨迹的方程。上述方法在实现四足机器人在通过躯干侧方向上运动增加其稳定性的同时，保证躯干一直沿前进方向向前运动且运动的速度、加速度均是连续的；在躯干运动轨迹的规划中，通过增加可调整系数，实现躯干侧移量的灵活调整，不存在任何后退运动，有效地提高了机器人的平均运动速度。

Description

四足机器人连续躯干运动轨迹规划方法

技术领域

本发明涉及一种四足机器人连续躯干运动轨迹的规划方法，可使四足机器人使用静步态在崎岖地形上行走时连续地沿前进方向行进且能够灵活地调整躯干的侧向移动量，属于机器人控制领域。

背景技术

当使用静步态在崎岖度较高的地形上行走时，四足机器人可通过躯干在前进方向的侧方向上的移动增加稳定裕度，以提高其自身的地形适应能力。四足机器人躯干的侧向移动量仅用来增加机器人在行走过程中的稳定性，因而，如果躯干的侧移量过大，将降低机器人的能量利用率，但是如果躯干的侧移量过小，就不能有效地增加机器人的稳定欲度，从而影响机器人的地形适应性。而躯干运动轨迹的规划直接关系到躯干在运动过程中的侧移量。

中国专利文献CN105353760A给出了一种基于支撑域位置反馈的足式机器人机身摆动幅度规划方法。CN106695791A公开了一种四足机器人自由步态提出了一种躯干轨迹规划方法。中国硕士学位论文《仿生恐龙机器人的步态仿真及系统实现》为仿生四足恐龙机器人提出了一种使用非连续静步态时的躯干运动轨迹规划方法。

当机器人使用上述这些方法在崎岖地形上行走时，仅根据各支撑足的位置规划躯干运动轨迹，不能灵活地调整躯干的侧移量。

论文“Compliant quadruped locomotion over rough terrain”(2009IEEE/RSJinternational conference on Intelligent robots and systems.IEEE,2009:814-820.)中提出了一种“8”字形的躯干运动轨迹规划方法，当四足机器人使用该方法沿前进方向在复杂地形上行走时，其躯干存在后退运动，影响了机器人的平均运动速度与能量利用率。

发明内容

本发明为了解决现有方法的上述不足，提出一种四足机器人连续躯干运动轨迹规划方法，该方法使四足机器人用静步态在崎岖地形上行走时能够连续地沿前进方向行进，不存在后退运动且可灵活调整躯干的侧移量。

本发明的四足机器人连续躯干运动轨迹规划方法，包括以下步骤：

(1)将四足机器人在躯干摆动阶段各支撑足投影至平面坐标系{^PO}中；

(2)根据各支撑足在平面坐标系{^PO}中的投影位置及设定的能够改变躯干侧移量的可调整参数，确定躯干运动轨迹的关键路径点；

(3)根据确定的躯干运动轨迹的关键路径点以及躯干运动的边界条件，给出躯干运动轨迹的方程。

所述步骤(2)中躯干运动轨迹关键路径点的确定过程如下所述：

将按迈步顺序接下来要摆动的后足记为NSF(Next Swing Foot)，记此足的期望落足点为DF(Desired Foothold)，与NSF位于躯干同侧的前足记为IFF(Ipsilateral FrontFoot)，记与NSF位于躯干不同侧的两只足中的前足与后足分别为CFF(ContralateralFront Foot)和CHF(Contralateral Hind Foot)；假设这四只足的投影在{^PO}中的坐标分别为P_NSF(^Px_NSF，^Py_NSF),P_IFF(^Px_IFF，^Py_IFF),P_CFF(^Px_CFF，^Py_CFF),P_CHF(^Px_CHF，^Py_CHF),NSF期望落足点DF投影在坐标系{^PO}中的坐标为P_DF(^Px_DF，^Py_DF)；

三角形ΔP_I P_CFF P_CHF为NSF与IFF相对应的双支撑三角形，为了保证机器人在摆动足运动过程中稳定性，在躯干摆动阶段(BSP)中，以使躯干的重心向双支撑三角形的内心位置运动为躯干重心调整的目标；

设直线L_DF-CFF为连接点P_DF与点P_CFF的直线，L_IFF-CHF为连接点P_IFF与点P_CHF的直线。根据各点的坐标，得到直线L_DF-CFF与L_IFF-CHF的方程：

其中，k_DF-CFF与b_DF-CFF分别为L_DF-CFF的斜率和截距；k_IFF-CHF与b_IFF-CHF分别为L_IFF-CHF的斜率和截距；

记两条直线L_DF-CFF与L_IFF-CHF的交点为P_I(^Px_I,^Py_I),由上式得到点P_I在坐标系{^PO}的坐标值为：

其中，k_DF-CFF与b_DF-CFF分别为L_DF-CFF的斜率和截距；k_IFF-CHF与b_IFF-CHF分别为L_IFF-CHF的斜率和截距；

三角形ΔP_I P_CFF P_CHF为NSF与IFF相对应的双支撑三角形，由双支撑三角形各顶点的坐标，得到其内心P_C(^Px_C,^Py_C)：

其中，

记点P_I与点P_C的直线距离为D(P_I,P_C)，根据两点的坐标，得到D(P_I,P_C)的值为：

记L_I-C为连接点P_I和点P_C的直线，w_m(0<w_m≤1)为第m个步态周期中，躯干运动轨迹的可调整参数，并记点^BSPP_E(^BSPx_E，^BSPy_E)为双支撑三角形Δ_DST内位于直线L_I-C上满足下式的点：

其中，

其中，k_I-C与b_I-C分别为L_I-C的斜率和截距。

根据上式得到点^BSPP_E在坐标系{^PO}中的坐标为：

记^DF-CFFL_⊥为过点^BSPP_E且垂直于L_DF-CFF的直线，并记点^FSPP_E(^FSPx_E，^FSPy_E)为稳定区域内位于直线^DF-CFFL_⊥上且与直线L_DF-CFF垂直距离(记为^DF-CFFD_⊥(^FSPx_E，^FSPy_E))为S_min的点，即点^FSPP_E需满足下式：

根据上式得到点^FSPP_E在坐标系{^PO}中的坐标为：

其中，

所述步骤(3)中给出躯干运动轨迹的方程的具体过程为：

根据确定的躯干运动轨迹的关键路径点^BSPP_E和点^FSPP_E的坐标，给出单个步态周期中四足机器人躯干运动轨迹的方程；

在躯干摆动阶段(BSP)中，坐标系{^PO}的原点为躯干重心移动的起点，点^BSPP_E为终点；在足的摆动阶段(FSP)中，^BSPP_E为躯干重心移动的起点，点^FSPP_E为终点，以下分别给出躯干摆动阶段和足的摆动阶段中的躯干运动轨迹方程；

A.躯干摆动阶段(BSP)中躯干运动轨迹方程：

记在躯干摆动阶段中机器人躯干运动轨迹的方程为：

其中，

T_BSP代表机器人完成BSP的规划时间。

在躯干摆动阶段中，坐标系{^PO}的原点与躯干重心在水平地面上的投影重合，为躯干重心移动的起点；运动结束之后，躯干重心的投影将到达点^BSPP_E，即躯干运动轨迹的位置边界条件满足：

在FSP阶段中机器人躯干仅沿前进方向上做匀速直线运动，而BSP相邻各有一个FSP，因而机器人在^Py方向上的躯干运动轨迹的速度和加速度边界条件为：

根据躯干运动的边界条件，得到躯干运动轨迹的方程^BSPf_y(t)的系数为：

其中，

将得到的系数代入躯干运动轨迹方程中，即得到四足机器人在运动过程中的躯干运动轨迹；

B.足的摆动阶段(FSP)中躯干运动轨迹方程：

在足的摆动阶段(FSP)中，机器人的躯干仅沿前进方向做匀速直线运动，由此得到机器人在足的摆动阶段(FSP)中，沿^Px方向上的躯干运动轨迹方程为：

其中，t∈[T_BSP,T_BSP+T_FSP]，T_FSP代表机器人完成FSP阶段的规划时间。

上述方法根据连续静步态的特点，并基于五次曲线给出躯干运动轨迹规划方法，实现四足机器人在通过躯干侧方向上运动增加其稳定性的同时，保证躯干一直沿前进方向向前运动且运动的速度、加速度均是连续的；在躯干运动轨迹的规划中，通过增加可调整系数，实现躯干侧移量的灵活调整。

本发明具有以下特点：

1.提出了一种四足机器人连续躯干运动轨迹，能够保证机器人在行走过程中躯干运动的连续性，以提高机器人的稳定性；

2.通过参数的调整，可灵活地改变躯干的侧移量，以通过躯干侧移量的调整，实现在保证四足机器人稳定性的前提下，提高能量利用率；

3.当使用所提出的方法时，四足机器人能够连续地沿前进方向向前运动，不存在任何后退运动，有效地提高了机器人的平均运动速度。

附图说明

图1为十二自由度四足仿生机器人的仿真模型图。

图2为坐标系{^PO}的示意图。

图3为4号足为NSF时的躯干运动轨迹关键路径点示意图。

图4为3号足为NSF时的躯干运动轨迹关键路径点示意图。

图5双支撑三角形内稳定裕度的分布的示意图。

具体实施方式

以如图1所示的十二自由度四足机器人为例，对本发明提出的一种四足机器人连续躯干运动轨迹规划方法作详细描述。

将四足机器人使用静步态行走时，按照选定的迈步顺序4-2-3-1(后左-前左-后右-前右，足的编号如图1所示)依此完成四只足的迈步运动定义为一个步态周期，则机器人的整个运动过程可分为多个步态周期。每个步态周期又可分为躯干摆动(Body SwayPhase,BSP)阶段与足的摆动阶段(Foot Sway Phase,FSP)。在BSP中，躯干通过侧向移动以增加其稳定裕度，以保证四足机器人在迈步过程中的稳定性；而在FSP阶段中，四足机器人的躯干做匀速直线运动，在增加机器人运动速度的同时，可以防止机器人因躯干运动速度或加速度过大的波动导致躯干向摆动足倾翻。

在本发明给出的躯干运动轨迹规划方法中，首先将四只足的位置投影到坐标系{^PO}中，如图2所示；然后根据各足的投影位置确定躯干移动轨迹关键路径点，最后，根据确定的路径点得到躯干运动轨迹方程。

1.躯干运动轨迹关键路径点的确定

为了方便描述躯干运动轨迹的规划过程，将机器人在四足支撑阶段的足重新命名，如图3和图4所示。

将按迈步顺序接下来要摆动的后足记为NSF(Next Swing Foot)，记此足的期望落足点为DF(Desired Foothold)，与NSF位于躯干同侧的前足记为IFF(Ipsilateral FrontFoot)，记与NSF位于躯干不同侧的两只足中的前足与后足分别为CFF(ContralateralFront Foot)与CHF(Contralateral Hind Foot)。假设这四只足的投影在{^PO}中的坐标分别为P_NSF(^Px_NSF，^Py_NSF),P_IFF(^Px_IFF，^Py_IFF),P_CFF(^Px_CFF，^Py_CFF),P_CHF(^Px_CHF，^Py_CHF),NSF期望落足点DF投影在坐标系{^PO}中的坐标为P_DF(^Px_DF，^Py_DF)。

三角形ΔP_I P_CFF P_CHF为NSF与IFF相对应的双支撑三角形，图5给出了在双支撑三角形(Double Support Triangle,DST)内机器人稳定裕度的分布示意图。由图5中可看出，当机器人躯干重心的投影位于DST的内心时，机器人可获得较大的稳定欲度，因此，为了保证机器人在摆动足运动过程中稳定性，在BSP中，以使躯干的重心向DST的内心位置运动为躯干重心调整的目标。

设直线L_DF-CFF为连接点P_DF与点P_CFF的直线，L_IFF-CHF为连接点P_IFF与点P_CHF的直线。根据各点的坐标，可得到直线L_DF-CFF与L_IFF-CHF的方程为：

其中，k_DF-CFF与b_DF-CFF分别为L_DF-CFF的斜率和截距；k_IFF-CHF与b_IFF-CHF分别为L_IFF-CHF的斜率和截距。

记两条直线L_DF-CFF与L_IFF-CHF的交点为P_I(^Px_I,^Py_I),由上式可得点P_I在坐标系{^PO}的坐标值为：

其中，k_DF-CFF与b_DF-CFF分别为L_DF-CFF的斜率和截距；k_IFF-CHF与b_IFF-CHF分别为L_IFF-CHF的斜率和截距。

三角形ΔP_I P_CFFP_CHF为NSF与IFF相对应的双支撑三角形。由双支撑三角形各顶点的坐标，可得其内心P_C(^Px_C,^Py_C)为：

其中，

记点P_I与点P_C的直线距离为D(P_I,P_C)，根据两点的坐标，可得D(P_I,P_C)的值为：

记L_I-C为连接点P_I和点P_C的直线，w_m(0<w_m≤1)为第m个步态周期中，躯干运动轨迹的可调整参数，并记点^BSPP_E(^BSPx_E，^BSPy_E)为双支撑三角形Δ_DST内位于直线L_I-C上满足下式的点。

其中，

其中，k_I-C与b_I-C分别为L_I-C的斜率和截距。

根据上式可得点^BSPP_E在坐标系{^PO}中的坐标为：

记^DF-CFFL_⊥为过点^BSPP_E且垂直于L_DF-CFF的直线，并记点^FSPP_E(^FSPx_E，^FSPy_E)为稳定区域内位于直线^DF-CFFL_⊥上且与直线L_DF-CFF垂直距离(记为^DF-CFFD_⊥(^FSPx_E，^FSPy_E))为S_min的点。即点^FSPP_E需满足下式：

根据上可得点^FSPP_E在坐标系{^PO}中的坐标为：

其中，

基于确定的点^BSPP_E和点^FSPP_E的坐标，接下来将给出单个步态周期中四足机器人躯干运动轨迹的方程。

2.躯干运动轨迹方程

为保证机器人躯干在BSP阶段中运动过程中的连续性，本发明提出了基于五次曲线的躯干轨迹生成方法。在BSP中，坐标系{^PO}的原点为躯干重心移动的起点，点^BSPP_E为终点；在FSP中，^BSPP_E为躯干重心移动的起点，点^FSPP_E为终点。在本部分中将分别给出BSP和FSP中的躯干运动轨迹方程。

(1)BSP中躯干运动轨迹方程

记在BSP中机器人躯干运动轨迹的方程为：

其中，

T_BSP代表机器人完成BSP的规划时间。

在BSP中，坐标系{^PO}的原点与躯干重心在水平地面上的投影重合，为躯干重心移动的起点；运动结束之后，躯干重心的投影将到达点^BSPP_E，即躯干运动轨迹的位置边界条件满足：

在FSP阶段中机器人躯干仅沿前进方向上做匀速直线运动，而BSP相邻各有一个FSP，因而机器人在^Py方向上的躯干运动轨迹的速度和加速度边界条件为：

然后，根据躯干运动的边界条件，可求得到躯干运动轨迹的方程^BSPf_y(t)的系数为：

其中，

最后，将得到的系数代入躯干运动轨迹方程中，即可得到四足机器人在运动过程中的躯干运动轨迹。

(2)FSP中躯干运动轨迹方程

在FSP中，机器人的躯干仅沿前进方向做匀速直线运动，由此可得机器人在FSP中，沿^Px方向上的躯干运动轨迹方程为：

其中，t∈[T_BSP,T_BSP+T_FSP]，T_FSP代表机器人完成FSP阶段的规划时间。

Claims (2)

1.一种四足机器人连续躯干运动轨迹规划方法，其特征是，包括以下步骤：

(1)将四足机器人在躯干摆动阶段各支撑足投影至平面坐标系{^PO}中；

(2)根据各支撑足在平面坐标系{^PO}中的投影位置及设定的能够改变躯干侧移量的可调整参数，确定躯干运动轨迹的关键路径点；

(3)根据确定的躯干运动轨迹的关键路径点以及躯干运动的边界条件，给出躯干运动轨迹的方程；

所述步骤(2)中躯干运动轨迹关键路径点的确定过程如下：

将按迈步顺序接下来要摆动的后足记为NSF，记此足的期望落足点为DF，与NSF位于躯干同侧的前足记为IFF，记与NSF位于躯干不同侧的两只足中的前足与后足分别为CFF和CHF；假设这四只足的投影在{^PO}中的坐标分别为P_NSF(^Px_NSF，^Py_NSF),P_IFF(^Px_IFF，^Py_IFF),P_CFF(^Px_CFF，^Py_CFF),P_CHF(^Px_CHF，^Py_CHF)，NSF期望落足点DF投影在坐标系{^PO}中的坐标为P_DF(^Px_DF，^Py_DF)；

三角形△P_IP_CFFP_CHF为NSF与IFF相对应的双支撑三角形，为了保证机器人在摆动足运动过程中的稳定性，在躯干摆动阶段中，以使躯干的重心向双支撑三角形的内心位置运动为躯干重心调整的目标；以BSP表示躯干摆动阶段，以FSP表示足的摆动阶段；

设直线L_DF-CFF为连接点P_DF与点P_CFF的直线，L_IFF-CHF为连接点P_IFF与点P_CHF的直线；根据各点的坐标，得到直线L_DF-CFF与L_IFF-CHF的方程：

其中，k_DF-CFF与b_DF-CFF分别为L_DF-CFF的斜率和截距；k_IFF-CHF与b_IFF-CHF分别为L_IFF-CHF的斜率和截距；

记两条直线L_DF-CFF与L_IFF-CHF的交点为P_I(^Px_I,^Py_I),由上式得到点P_I在坐标系{^PO}的坐标值为：

由双支撑三角形各顶点的坐标，得到其内心P_C(^Px_C,^Py_C)：

其中，

记点P_I与点P_C的直线距离为D(P_I,P_C)，根据两点的坐标，得到D(P_I,P_C)的值为：

记L_I-C为连接点P_I和点P_C的直线，w_m为第m个步态周期中，躯干运动轨迹的可调整参数，0<w_m≤1，并记点^BSPP_E(^BSPx_E，^BSPy_E)为双支撑三角形△_DST内位于直线L_I-C上满足下式的点：

其中，

其中，k_I-C与b_I-C分别为L_I-C的斜率和截距；

根据上式得到点^BSPP_E在坐标系{^PO}中的坐标为：

记^DF-CFFL_⊥为过点^BSPP_E且垂直于L_DF-CFF的直线，并记点^FSPP_E(^FSPx_E，^FSPy_E)为稳定区域内位于直线^DF-CFFL_⊥上且与直线L_DF-CFF垂直距离记为^DF-CFFD_⊥(^FSPx_E，^FSPy_E)为S_min的点，即点^FSPP_E需满足下式：

根据上式得到点^FSPP_E在坐标系{^PO}中的坐标为：

其中，

2.根据权利要求1所述的四足机器人连续躯干运动轨迹规划方法，其特征是，所述步骤(3)中给出躯干运动轨迹的方程的具体过程为：

根据确定的躯干运动轨迹的关键路径点^BSPP_E和点^FSPP_E的坐标，给出单个步态周期中四足机器人躯干运动轨迹的方程；

在躯干摆动阶段中，坐标系{^PO}的原点为躯干重心移动的起点，点^BSPP_E为终点；在足的摆动阶段中，^BSPP_E为躯干重心移动的起点，点^FSPP_E为终点，以下分别给出躯干摆动阶段和足的摆动阶段中的躯干运动轨迹方程；

A.躯干摆动阶段中躯干运动轨迹方程：

记在躯干摆动阶段中机器人躯干运动轨迹的方程为：

其中，

T_BSP代表机器人完成躯干摆动阶段的规划时间；

在躯干摆动阶段中，坐标系{^PO}的原点与躯干重心在水平地面上的投影重合，为躯干重心移动的起点；运动结束之后，躯干重心的投影将到达点^BSPP_E，即躯干运动轨迹的位置边界条件满足：

在足的摆动阶段中机器人躯干仅沿前进方向上做匀速直线运动，而躯干摆动阶段相邻各有一个足的摆动阶段，因而机器人在^Py方向上的躯干运动轨迹的速度和加速度边界条件为：

根据躯干运动的边界条件，得到躯干运动轨迹的方程^BSPf_y(t)的系数为：

其中，

将得到的系数代入躯干运动轨迹方程中，即得到四足机器人在运动过程中的躯干运动轨迹；

B.足的摆动阶段中躯干运动轨迹方程：

在足的摆动阶段中，机器人的躯干仅沿前进方向做匀速直线运动，由此得到机器人在足的摆动阶段中，沿^Px方向上的躯干运动轨迹方程为：

其中，t∈[T_BSP,T_BSP+T_FSP]，T_FSP代表机器人完成足的摆动阶段的规划时间。

Images