[发明专利]四足机器人连续躯干运动轨迹规划方法

权利要求书

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个步态周期中，躯干运动轨迹的可调整参数，0w_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代表机器人完成足的摆动阶段的规划时间。