(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210539272.3 (22)申请日 2022.05.18 (71)申请人 山东交通学院 地址 250357 山东省济南市长清区长清大 学科技园海棠路5 001号 (72)发明人 辛亚先　荣学文　李贻斌　阮久宏　 范永　 (74)专利代理 机构 济南领升专利代理事务所 (普通合伙) 37246 专利代理师 王吉勇 (51)Int.Cl. B25J 9/16(2006.01) B25J 11/00(2006.01) G05D 1/08(2006.01) (54)发明名称 双轮腿-臂机 器人全身运动规划与控制方法 (57)摘要 本发明公开了一种双轮腿 ‑臂机器人全身运 动规划与控制方法， 针对双轮腿 ‑臂机器人躯干、 手臂协同运动以及手臂末端力输出需求， 提出了 基于分层优化的多任务空间规划方法， 解决了全 身运动规划冗余问题。 基于二级桌子 ‑小车模型 对轮腿‑躯干‑手臂力传递进行分析， 将手臂运动 与作业虚拟广义力通过前馈补偿的形式纳入躯 干广义力规划。 分析左右轮腿力分配特点， 通过 构造提出了基于二次规划的自平衡全身力矩控 制器， 将各层非线性约束显性化， 从而解算出获 得满足全身动态平衡的轮腿力矩。 通过该力矩的 施加所生 成的轮腿姿态能够保持双轮腿 ‑臂机器 人处于动态平衡点， 提高了运动作业的动态稳定 性。 权利要求书3页 说明书11页 附图2页 CN 114700955 A 2022.07.05 CN 114700955 A 1.一种双轮腿 ‑臂机器人全身运动规划与控制方法， 包括： (1)在双轮腿系统力分析的基础上进行了双轮腿 ‑臂机器人动力学分析， 将手臂的运动 与作业对机器人产生的稳定性影响映射 为躯干广义力的一部分， 进行统一 规划与处 理； (2)根据实际需求所确定的手臂末端、 躯干位姿任务空间， 提出多任务空间分层优化方 法， 解决双轮腿 ‑臂机器人全身运动规划的冗余， 同时实现多个具有不同优先级任务空间的 参考轨迹规划， 操纵目标物的同时驱动自身运动； (3)手臂与环境的交互力对双轮腿 ‑臂机器人的动态稳定产生实时影响， 以此交互力为 控制目标， 构造前馈项补偿项加入到全身力矩控制器中， 补偿手臂末端广义力对全身关节 力矩所产生的影响， 从而实现对操作物动态力的前馈广义力补偿， 增 加了全身稳定 鲁棒性。 2.如权利要求1所述的双轮腿 ‑臂机器人全身运动规划与控制方法， 其特征是， 所述双 轮腿‑臂机器人动力学分析， 在双轮腿系统力分析的基础上， 将 手臂杆件与末端 所受外力集 中于一点， 形成以躯干为基座的第二级桌子 ‑小车模型， 将躯干作为轮腿 ‑手臂广义力传递 的桥梁， 将第二级小车模型所需广义力映射到第一级小车模型即躯干上去， 作为外力进行 前馈补偿， 然后， 进行轮腿力矩解算， 实现手臂运动与作业时的双轮腿 ‑臂机器人动态稳定 运动。 3.如权利要求1所述的双轮腿 ‑臂机器人全身运动规划与控制方法， 其特征是， 所述多 任务空间分层优化方法， 双轮腿 ‑臂机器人 的控制目标为单一任务空间的运动 或者同时提 出两个任务空间的期望轨迹； 两个任务空间并不是完全解耦的， 存在相互制约的关系， 因 此， 并不能满足任意期望轨迹的执行； 为了 保证控制器所接收执行信号的有效性， 构 造上层 规划器， 根据不同工况 的实际情况进行任务空间优先级分层， 采用优化方法规划出各任务 空间的可行参考轨迹； 根据分层优化算法， 将最底层任务空间的优化输出作为广义关节加 速度对速度与位置进行 更新， 得到参 考轨迹。 4.如权利要求1所述的双轮腿 ‑臂机器人全身运动规划与控制方法， 其特征是， 所述全 身力矩控制器， 将左右轮腿的力与力矩解算问题转化为二次规划解算问题(Quadratic   Programming， QP)， 定义该QP解 算的变量 为： x＝[Frx Frz Nry Flx Flz Nly frα flα τr0 τr1 τr2 τl0 τl1 τl2]T 其中， Fix、 Fiz、 Niy分别为轮腿i与躯干铰接点处的水平力、 竖直力以及俯仰力矩， fiα为轮 腿i的轮地接触点处推动机器人航向运动的水平力； τi0τi1τi2分别为轮腿i的轮 关节力矩、 膝 关节力矩与髋关节力矩； 同时将力分配约束、 轮腿姿态约束、 关节力矩约束分别转化为二次规划的目标函数与 约束， 构成一个完整的具有凸约束的QP解 算问题： (1)目标函数构造： 轮腿‑躯干接触力与躯 干所需广义力应满足： b1＝A1x 其中， b1＝Wb′， A1＝[Jb′  06×6]， 其中， Wb′为期望广义力矩阵， Jb′为轮腿‑躯干铰接点处 的广义力向躯 干质心映射的雅克比矩阵的扩展形式； 力矩 τ ＝[ τr0 τr1 τr2 τl0 τl1 τl2]T解算部分应满足： b2＝A2x权　利　要　求　书 1/3 页 2 CN 114700955 A 2其中， 其中， Gi、 Ii、 Ji分别为轮 腿i的重力项、 初始惯性力矩阵以及末端力雅克比矩阵； 而Jwi为两连 杆腿部雅克比矩阵， Jwi(1)为取矩阵Jwi的第一列， 将以上力分布部分与关节力矩解 算部分合并为 一个QP解 算问题： Ax＝b 其中， (2)约束构造： 将力分配约束公式整理为如下 形式： x(0)x(4) ‑x(1)x(3)＝0 将轮腿的伸展长度需要满足以下约束： 其中， Lx为髋关节与轮地接触点的水平距离， Lz为髋关节垂直高度， Fx与Fz为躯干受到的 二维合力； L、 分别为髋关节与轮地接触点的最小允许距离与最大允许距离； 将该约束 整理为两个单边约束： 同时， 因为 地面只能提供支持力， 而不能提供吸引力， 因此纵向力Fz必须为正， 即： x(1)≥0 x(4)≥0 为保证所计算的各关节力矩在关节电机 输出扭矩范围 之内， 使得： 至此， 能够构成一个完整的具有凸约束的QP解 算问题： 权　利　要　求　书 2/3 页 3 CN 114700955 A 3