(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202111560356.7 (22)申请日 2021.12.20 (65)同一申请的已公布的文献号 申请公布号 CN 114248855 A (43)申请公布日 2022.03.29 (73)专利权人 北京理工大 学 地址 100081 北京市海淀区中关村南大街5 号 (72)发明人 黄强　韩连强　陈学超　余张国　 石青　高志发　张锦涛　 (74)专利代理 机构 南京智造力知识产权代理有 限公司 32382 专利代理师 汪芬 (51)Int.Cl. B62D 57/032(2006.01) G06F 30/20(2020.01) G06T 3/40(2006.01)(56)对比文件 CN 110405762 A,2019.1 1.05 CN 111176342 A,2020.0 5.19 CN 10834521 1 A,2018.07.31 CN 103042526 A,2013.04.17 CN 1098078 84 A,2019.0 5.28 JP 2011194503 A,2011.10.06 JP 2008229747 A,20 08.10.02 CN 111230868 A,2020.0 6.05 CN 112631277 A,2021.04.09 CN 113050645 A,2021.0 6.29 CN 110286679 A,2019.09.27 US 2015120 044 A1,2015.04.3 0 US 202123726 5 A1,2021.08.0 5 CN 112051741 A,2020.12.08 CN 112882467 A,2021.0 6.01 CN 110315543 A,2019.10.1 1 审查员 周晓龙 (54)发明名称 双足机器人空间域 步态规划与控制的方法 (57)摘要 本发明公开了双足机器人空间域步态规划 与控制的方法， 将双足机器人髋关节相对支撑腿 踝关节的水平距离设为相变量δ， 通过当前状 态、 参考步长及参考速度设计优化出基于相变量 的摆动腿关节 参考角度轨迹； 完成空间域的摆动 腿步态规划； 建立二阶倒立摆模型， 通过非线性 模型预测控制优化出用于速度控制的支撑腿髋 关节参考力矩； 基于获得上述的摆动腿关节参考 角度轨迹以及支撑腿髋关节参考力矩之后， 利用 全身动力学Quadratic  Programming控制在多目 标任务中优化出关节力矩， 进而控制机器人步态 行走。 本方法将摆动腿踝关节的运动与时间脱离 开来， 使其仅依赖自身状态， 增强鲁棒 性。 权利要求书3页 说明书8页 附图2页 CN 114248855 B 2022.10.21 CN 114248855 B 1.双足机器人空间域步态规划与控制的方法， 其特征在于， 将双足机器人髋关节相对 支撑腿踝关节的水平距离 设为相变量δ， 通过当前状态、 参考步长及参考速度设计优化出基 于相变量的摆动腿关节参 考角度轨 迹； 完成空间域的摆动腿步态规划； 以支撑腿脚踝为 坐标原点构建二阶倒立摆 模型， 表示 为： 其中， Mip表示动力学的惯性矩阵， Nip表示动力学的科氏力矩阵， Gip表示动力学的重力 向量， Bip表示力矩筛选矩阵， u是二阶倒立摆两个连杆连接关节的驱动力矩； 表示模型速 度向量， 表示模型加速度向量， 通过非线性模型预测控制优化出用于速度控制的支撑腿 髋关节参 考力矩； 基于获得上述的摆动腿关节参考角度轨迹以及支撑腿髋关节参考力矩之后， 利用全身 动力学QP控制在多目标任务中优化出关节力矩， 进 而控制机器人步态行 走； 空间域的摆动腿步态规划如下： S1、 设定髋关节相对支撑腿脚踝的水平距离xh‑a为相变量δ＝xh‑a， 记开始时的xh‑a为 期望xh‑a能够到达参考步长 的一半， 因此归一 化后的相变量的取值s表示 为： S2、 在x方 向上， 使用五次插值方法规划， 设s是变量， 让摆动腿脚踝x轴坐标xf从上一步 长 连续变化到达 当s＝0和s＝1时曲线的速度和加速度为零， 从而得到参考的摆动 腿脚踝x轴坐标轨 迹 及其速度 S3、 在z方向上， 通过优化贝塞尔曲线参数来规划参考的摆动腿脚踝z轴坐标 轨 迹； 设优化变量为ξ＝[α0 α1…αM]T， 通过建立二次优化问题， 来获得符合要求的贝塞尔曲 线， 其中， αM为第M+1个系数。 2.根据权利要求1所述的双足机器人空间域步态规划与控制的方法， 其特征在于， 摆动 腿脚踝z轴坐标 轨迹表示为： 摆动腿脚踝z轴坐标速度 为： 其中， M是贝 塞尔曲线的维度， αk为系数， k 为求和运算迭代变量， ( ·)！ 表示阶乘运 算。 3.根据权利要求1所述的双足机器人空间域步态规划与控制的方法， 其特征在于， 所构 建建立二次优化问题 表示为： 权　利　要　求　书 1/3 页 2 CN 114248855 B 2ξ >0 其中， 为当s＝ ε 时摆动腿脚踝的期 望高度， 优化目标是使其约等于期 望的最大抬 脚高度 优化问题的约束包括当s＝0时位置和速度分别为零， 当s＝1时位置和速度也分 别为零。 4.根据权利要求1所述的双足机器人空间域步态规划与控制的方法， 其特征在于， 使用 非线性模型预测控制得到控制 速度的关节驱动力矩u， 设控制周期时间为Δt， 优化变量设 为 (·)T运算表示求向量或矩阵的转置， 其中 分别 是t0、 t0+Δt、 t0+nΔt时刻对应的关节驱动力矩， 计数量n＝T/Δt； 设定非线性优化目标如 下： 其中||·||2表示2范数运算， Wv和Wu分别是两个范数运算的权重矩阵； 同时优化问题的 约束条件分别如下： 其中(·)k的上角标表示该量在第k次计数时 的数值， 和 分别是该计数下二阶倒 立摆模型的第二级摆与竖直方向的最大和最小角度， 和 分别是该计数下关节驱动 力矩最大和最小值， 最终优化得到的结果中 其他量将被舍弃。 5.根据权利要求1所述的双足机器人空间域步态规划与控制的方法， 其特征在于， 利用 全身动力学QP控制在多目标任务中优化出关节力矩的方法包括如下步骤： S1、 双足机器人全身动力学的浮动基模型表示 为： 其中， qs表示机器 人广义坐标系的位置， 是qs的微分向量， 分别表示速度和加速度； Ms,Cs,Gs分别表示动力学的惯性矩阵、 科氏力矩阵和重力向量， τ表示驱动力矩向量， Bs表示 力矩筛选矩阵； 分别表示右腿和左腿末端受到 的外界力， 而Jr,Jl表示右腿和左腿末 端在世界坐标系中的雅可比矩阵； S2、 构造QP问题， 设优化变量包括模型加速度 驱动力矩τ和右左脚末端的外界力 则优化的目标函数表示 为： 权　利　要　求　书 2/3 页 3 CN 114248855 B 3