(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210500598.5 (22)申请日 2022.05.09 (71)申请人 中兵智能创新研究院有限公司 地址 100071 北京市丰台区科兴 路7号1层A 区21号 (72)发明人 江磊　苏波　许鹏　姚其昌　 蒋云峰　梁振杰　郭亮　赵建新　 邱天奇　慕林栋　 (74)专利代理 机构 中国兵器 工业集团公司专利 中心 11011 专利代理师 周恒 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种四足机 器人姿态预适应控制方法 (57)摘要 本发明属于机器人运动控制技术领域， 具体 涉及一种四足机器人姿态预适应控制方法。 该方 法包括： 基于局部障碍物高程信息计算安全的落 足点； 基于落足点信息及本体传感信息预估地形 姿态角度； 根据地形姿态角度建立机器人姿态预 适应控制器。 本发明通过建立机器人姿态预适应 控制器实现对地形的稳定适应， 从而提升机器人 在复杂路面下的通过性能。 权利要求书3页 说明书6页 附图1页 CN 114683289 A 2022.07.01 CN 114683289 A 1.一种四足机器人姿态预适应控制方法， 其特 征在于， 所述方法包括如下步骤： 步骤1： 基于局部障碍物高程信息计算 安全的落足点； 步骤2： 基于落足点信息及本体传感信息预估地形姿态角度； 步骤3： 根据地形姿态角度建立机器人姿态预适应控制器。 2.如权利要求1所述的四足机器人姿态预适应控制方法， 其特 征在于， 所述 步骤1包括： 步骤11： 利用机载的雷达、 摄像头传感器采集落足点区域附近的局部地图， 并获取局部 地图内包 含的局部障碍物高程信息； 步骤12： 利用局部障碍物高程信息检测落足安全区域； 步骤13： 根据局部地图的原点、 落足安全区域的高程信 息及地图网格尺寸， 计算安全的 落足点。 3.如权利要求2所述的四足机器人姿态预适应控制方法， 其特 征在于， 所述 步骤2包括： 步骤21： 基于包括关节角度传感器、 IMU的本体传感信息， 实时计算足端位置； 所述足端 位置包括当前支撑腿的实时足端位置和摆动腿结束支撑时的足端位置； 步骤22： 采用当前支撑腿的实时足端位置、 摆动腿结束支撑时的足端位置、 摆动腿的落 足点信息的位置预估地形姿态角度。 4.如权利要求3所述的四足机器人姿态预适应控制方法， 其特 征在于， 所述 步骤3包括： 步骤31： 基于预估的地形姿态角度， 建立姿态预适应角度； 步骤32： 将姿态预适应角度作为输入， 建立机身虚拟伺服控制器， 并通过目标优化方法 分配到足部； 步骤33： 建立底层柔 顺伺服控制器。 5.如权利要求4所述的四足机器人姿态预适应控制方法， 其特征在于， 所述步骤11中， 利用机载的雷达、 摄像头传感器采集落足点区域附近的局部地图， 及局部地图内包含的局 部障碍物高程信息 。 6.如权利要求5所述的四足机器人姿态预适应控制方法， 其特征在于， 所述步骤12中， 利用局部障碍物高程信息计算 地形起伏信息σ， σ 计算如下： 式中， N为高程网格的数量， xi为第i个高程网格的高程值， 为N个高程网格的高程平均 值； 设定地形起伏信息阈值σ*， 当前起伏信息σ 小于设定阈值σ*时， 认为为落足安全区域， 记 录安全落足区域的高程xσ以及网格数量cx、 cy。 7.如权利要求6所述的四足机器人姿态预适应控制方法， 其特征在于， 所述步骤13中， 根据局部地图的原点O、 落足安全区域的高程信息xσ及地图网格尺寸Δc， 计算安全的落足 点pσ； pσ＝[Ox+cxΔc,Oy+cyΔc,xσ]T 其中， Ox、 Oy为局部地图原点O的元 素。 8.如权利要求7所述的四足机器人姿态预适应控制方法， 其特征在于， 所述步骤21中， 基于关节传感器、 IMU实时计算足端位置p， 首先根据腿部运动学计算足端相对于机身中心权　利　要　求　书 1/3 页 2 CN 114683289 A 2的位置pb， 再根据IMU采集的侧倾、 俯仰、 偏航姿态角度， 计算姿态变换矩阵R， 从而得到足端 位置p； p＝Rpb。 9.如权利要求8所述的四足机器人姿态预适应控制方法， 其特征在于， 所述步骤22中， 采用当前支撑腿的实时足端位置ps、 摆动腿结束支撑时的足端位置pw、 摆动腿的落 足点pσ的 位置预估地形姿态角度， 包括翻滚姿态角度 及俯仰姿态角度γg， 通过计算足端位置p构 成的平面法向量 n进行求解， 法向量 求解如下： (p‑p0)·n＝0 式中， p0为初始位置 。 10.如权利要求9所述的四足机器人姿态预适应控制方法， 其特征在于， 所述步骤31中， 基于预估的地形姿态角度， 引入比例因子， 建立姿态预适应角度， 建立如下： γd＝kγγg 式中， kγ分别为侧倾角、 俯仰角适应比例因子， γg为预估的地形角度， γd为 期望的侧倾角、 俯仰角； 所述步骤32中， 将姿态预适应角度作为输入， 建立机身虚拟伺服控制器， 并通过目标优 化方法分配到足部， 机身虚拟伺服控制器建立如下： 式中， Mm、 Fm为机体虚拟合力矩矢量、 虚拟合力矢量； kp为正定增益矩阵； q为机身实际的 位姿矢量； qd为机身期望的位姿矢量， γd为qd的前两个元素； kv为正定微分系数矩阵； 为机身实际的位姿速度矢量， 为机身期望的位姿速度矢量， kff为正定速度前馈系数矩 阵， m为机器人质量， g为重力加速度； 通过设定约束条件， 建立目标优化函数F(x)， 求解满足约束条件， 且使目标函数获得最 小值的一组解， 目标优化 函数如下： min F(x)＝(Ax ‑b)TS(Ax‑b)+α xTWx+β ||x‑x*|| 其中， 式中， S为加权矩阵,W 为半正定对称矩阵, α 、 β 为整定因子.x*为上一时刻的优化 解； 约束条件如下： 上式中， 为第i接触点的法向力； 为第i接触点的切向力； μ为滑动摩擦系数； 所述步骤33中， 以分配足部的力作为输入， 建立关节柔 顺伺服控制器， 设计如下 所示：权　利　要　求　书 2/3 页 3 CN 114683289 A 3