(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211012725.3 (22)申请日 2022.08.23 (71)申请人 中国人民解 放军63653部队 地址 841700 新疆维吾尔自治区乌鲁 木齐 市21信箱E19分箱 (72)发明人 李增彦　邓金球　付良瑞　朱宝良　 代佳凯　廖旭东　陈涛　陶宏博　 潘荣敏　 (74)专利代理 机构 石家庄国为知识产权事务所 13120 专利代理师 刘少卿 (51)Int.Cl. B25J 11/00(2006.01) B25J 9/16(2006.01) B25J 13/08(2006.01)B25J 18/00(2006.01) (54)发明名称 一种工程作业机器人防倾覆预警装置及方 法 (57)摘要 本发明提供了一种工程作业机器人防倾覆 预警装置及方法， 属于工程机械安全 预警技术领 域。 本发明提供的一种工程作业机器人防倾覆预 警装置， 包括压力传感器、 角度传感器、 距离传感 器、 电液比例阀、 控制器及终端机， 压力传感器用 于监测关节油缸的压力； 角度传感器用于进行各 组件角度的监测； 距离传感器用于扫描底盘在地 面的状态； 电液比例阀设置在液压油路总阀处； 控制器用于接收压力传感器、 角度传感器及距离 传感器相关数据， 控制电液比例阀启动， 降低车 速并启动紧急制动； 终端机连接控制器显示各组 件的实时状态。 本发明提供的一种工程作业机器 人防倾覆预警装置及方法， 当预测到发生严重倾 覆风险时， 制动车辆， 改造成本低， 安全机制完 善。 权利要求书2页 说明书8页 附图2页 CN 115383758 A 2022.11.25 CN 115383758 A 1.一种工程作业机器人防倾覆预警装置， 其特 征在于， 包括： 压力传感器， 所述压力传感器设置在大臂 的关节油 缸的进油口与出油口， 用于监测关 节油缸进油口与出油口处的压力； 角度传感器， 所述角度传感器设置在工程作业机器人的驾驶室、 底盘、 推土铲、 大臂、 小 臂、 夹爪上， 用于进 行驾驶室、 底盘、 推土铲、 大臂、 小臂和夹爪角度的监测， 并且标定实际测 量的驾驶室、 底盘、 推土铲、 大臂、 小臂、 夹爪各部件之间角度的差值； 距离传感器， 所述距离传感器设置在工程作业机器人上， 用于扫描工程作业机器人在 地面的状态； 电液比例阀， 所述电液比例阀设置在工程作业机器人的液压油路总阀处； 控制器， 所述控制器设置在工程作业机器人上， 用于接收所述压力传感器、 所述角度传 感器以及所述距离传感器的数据， 所述控制器可控制所述电液比例阀开启或关闭， 以对工 程作业机器人进行制动； 以及 终端机， 所述终端机与所述控制器通过总线协议进行通信， 所述终端机可通过设置相 关参数， 显示各组件的实时状态。 2.如权利要求1所述的一种工程作业机器人防倾覆预警装置， 其特征在于， 所述终端机 通过所述控制器获取所述压力传感器、 所述角度传感器以及所述距离传感器相关数据， 通 过软件计算 目标重量、 倾覆力矩并评估倾覆风险， 所述终端机可控制所述控制 器降低车速 并启动紧急制动。 3.如权利要求1所述的一种工程作业机器人防倾覆预警装置， 其特征在于， 所述角度传 感器远离工程作业机器人的驾驶室、 底盘、 推土铲、 大臂、 小臂、 夹爪上的转动铰接部位设 置。 4.一种工程作业机器人防倾覆预警方法， 其特征在于， 使用了如权利要求1 ‑3任意一项 所述的工程作业机器人防倾覆预警装置， 包括如下步骤： S1、 建立工程作业机器人的坐标系， 其中坐标原点设定在工程作业机器人的旋转中心 在地面的投影处， 坐标系的x轴 方向指向工程作业机器人 的前进方向， 坐标系的y轴 方向垂 直于工程作业机器人的前进方向并指向前进 方向的左侧， 且x轴 与y轴位于同一水平面， z轴 方向垂直于水平面并指向上， 确定底盘、 推土铲、 驾驶室、 大臂、 小臂、 夹爪、 目标、 操作人员 的质量为mj， j＝1,2,3,4,5,6,7,8及质心Cj， j＝1,2,3,4,5,6,7,8； S2、 获取倾覆支点， 距离传感器对工程作业机器人与地面接触的底盘部分进行逐点扫 描， 定义扫描得到的距离为d， 根据 距离传感器的坐标以及距离d 的长度与角度， 得到与地面 接触点的坐标， 通过与地面接触点的坐标值确定工程作业机器人 的四个支点的位置； 四个 支点组成四边形， 标定相邻的两个支点的x轴坐标与y轴坐标的中间值为倾覆点， 倾覆点位 于x轴与y轴的平面上， 从而确定前、 后、 左、 右四个方向的倾覆点分别为pj， j＝1,2,3,4； S3、 获取目标质量， 抓取到目标后， 控制小臂、 夹具垂直朝向地面方向， 根据角度传感 器、 压力传感器的数据综合评估目标质量； S4、 倾覆力矩计算， 通过各组件的质量以及各组件的角度， 计算得到各组件的质心的坐 标， 底盘以上部分的质量为mU， 质心为CU， 底盘及推土铲质量为mD， 质心为CD， 全部质量m为底 盘以上部分的质量mU与底盘及推土铲质量为mD的和， 定义 为底盘以上部 分与底盘间旋转权　利　要　求　书 1/2 页 2 CN 115383758 A 2的相对角度， mU、 mD、 相结合计算得到工程作业机器人整体的质心C， 根据计算得到的全部 质量m以及质心C计算得到 工程作业机器人的前、 后方向力矩与左、 右方向力矩； S5、 倾覆风险评估准则， 定义基准力矩为工程作业机器人位于稳定状态时的前、 后方向 力矩与左、 右方向力矩， 定义当前时刻力矩为此时状态下 的前、 后方向力矩与左、 右方向力 矩， 分别计算得到四个倾覆点的基准力矩以及当前时刻力矩， 定义同方向的基准力矩中最 小值为倾覆力矩， 倾覆点当前时刻力矩与基准力矩的比值 为工程作业机器人的倾覆概 率； S6、 倾覆风险预警阈值定义， 根据实时的倾覆概率， 可调整预警的阈值， 将其分为无、 低、 中、 高、 事故几个等级， 实现分级预警； S7、 基于运动预测的动态倾覆风险评估， 根据工程作业机器人当前的操控指令， 通过速 度、 角速度积分方式， 预测工程作业机器人的底盘、 各 组件的运动趋势， 预测1、 2、 4秒等自定 义时刻之后的平台姿态， 根据预测的平台姿态， 计算预测的倾覆力矩， 计算预测的倾覆概 率， 将自定义时刻后的倾覆风险预警结果及当前时刻操作动作指令， 发送至终端机的显示 屏； S8、 高风险作业的自动干预， 当工程作业机器人按照当前运动指令运动时， 检测到将进 入高风险倾覆状态时， 提供声光报警。 5.如权利要求4所述的一种工程作业机器人防倾覆预警方法， 其特征在于， 在步骤S2 中， 以与地面接触点的坐标的z(i)值的变化， 来判定支 点的位置， 考虑地面高度不平， 在规定 误差范围内， 可在小的阈值内认 为与地面接触点的坐标的z(i)值为0， z(i)值从负到正或从正 到负， 则支点位于z(i)值为0处及底盘一端， 如果z(i)值大于零， 则支点在底盘的两端， 如果 z(i)值位于中间时大于零， 位于两端时小于零， 则前侧支点与后侧支点分别位于z(i)值为0 处。 6.如权利要求4所述的一种工程作业机器人防倾覆预警方法， 其特征在于， 在步骤S3 中， 以大臂铰接在底盘的铰接点为支点， 根据力矩平衡原则， 大臂油缸的力矩等于大臂、 小 臂、 夹爪和目标质量的合力矩， 根据几何关系及大臂油缸、 大臂、 小臂、 夹爪的机械参数， 求 得目标质量。 7.如权利要求6所述的一种工程作业机器人防倾覆预警方法， 其特征在于， 定义组件的 质心与该组件的铰接点之 间的连线与该组件两个铰接点之 间的连线的夹角为固定偏差角， 通过固定偏差角以及各组件的长度等， 通过几何关系计算， 得到合力矩计算所需的相关长 度。 8.如权利要求4所述的一种工程作业机器人防倾覆预警方法， 其特征在于， 在步骤S5 中， 工程作业机器人 的稳定状态 时， 工程作业机器人 的各组件保持静态且位于默认收起初 态时为稳定状态。 9.如权利要求4所述的一种工程作业机器人防倾覆预警方法， 其特征在于， 在步骤S8 中， 检测到将进入高风险倾覆状态时， 控制器控制电液比例阀从而降低部件的运动速度。 10.如权利要求9所述的一种工程作业机器人防倾覆预警方法， 其特征在于， 在步骤S8 中， 检测到将进入高风险倾覆状态时， 控制器控制电液比例阀对工程作业机器人 的运动进 行锁定。权　利　要　求　书 2/2 页 3 CN 115383758 A 3