一种工程作业机器人防倾覆预警装置及方法与流程

1.本发明属于工程机械安全预警技术领域，具体涉及一种工程作业机器人防倾覆预警装置及方法。


背景技术：

2.工程作业机器人是一种常见的用于工程作业的器械，在工程作业机器人上常常设置有多自由度的液压抓取设备，工程作业机器人包括底盘、推土铲、驾驶室、大臂、小臂、夹爪、关节油缸，用于进行目标的抓取，当抓取负载高、作业半径大或在非结构化路面行进时，工程作业机器人极易发生倾覆风险。目前大多数的工程作业机器人既可以在驾驶舱内操作，也可以远程控制进行物体的抓取搬运，当需要抓取搬运的目标重量估计不准确或路面状况无法判断时，就有可能增大工程作业机器人发生倾覆的风险，必须采用相关技术或方法降低风险发生概率。现有技术中，通常在工程作业机器人出厂时添加相关传感器，利用力矩判断法进行倾覆判断，但是在以下几个方面存在问题：通常考虑的路况比较单一，仅支持工程作业机器人在平坦路面或者机械底盘的整体与路面接触的倾斜路面上行进，无法准确评估目标重量，无法预测进一步动作导致的倾覆风险，无法直观观察数据变化，无法在发生风险时紧急制动和报警。


技术实现要素：

3.本发明的目的在于提供一种工程作业机器人防倾覆预警装置及方法，旨在解决工程作业机器人作业路况单一、无法准确评估目标重量、无法预测进一步动作导致的倾覆风险、无法直观观察数据变化、无法在发生风险时紧急制动和报警的问题。
4.为实现上述目的，本发明采用的技术方案是：提供一种工程作业机器人防倾覆预警装置，包括：压力传感器，所述压力传感器设置在大臂的关节油缸的进油口与出油口，用于监测关节油缸进油口与出油口处的压力；
5.角度传感器，所述角度传感器设置在工程作业机器人的驾驶室、底盘、推土铲、大臂、小臂、夹爪上，用于进行驾驶室、底盘、推土铲、大臂、小臂和夹爪角度的监测，并且标定实际测量的驾驶室、底盘、推土铲、大臂、小臂、夹爪各部件之间角度的差值；
6.距离传感器，所述距离传感器设置在工程作业机器人上，用于扫描工程作业机器人在地面的状态；
7.电液比例阀，所述电液比例阀设置在工程作业机器人的液压油路总阀处；
8.控制器，所述控制器设置在工程作业机器人上，用于接收所述压力传感器、所述角度传感器以及所述距离传感器的数据，所述控制器可控制所述电液比例阀开启或关闭，以对工程作业机器人进行制动；以及
9.终端机，所述终端机与所述控制器通过总线协议进行通信，所述终端机可通过设置相关参数，显示各组件的实时状态。
10.在一种可能的实现方式中，所述终端机通过所述控制器获取所述压力传感器、所
述角度传感器以及所述距离传感器相关数据，通过软件计算目标重量、倾覆力矩并评估倾覆风险，所述终端机可控制所述控制器降低车速并启动紧急制动。
11.在一种可能的实现方式中，所述角度传感器远离工程作业机器人的驾驶室、底盘、推土铲、大臂、小臂、夹爪上的转动铰接部位设置。
12.本发明提供的一种工程作业机器人防倾覆预警装置的有益效果在于：
13.与现有技术相比，包括压力传感器、角度传感器、距离传感器、电液比例阀、控制器及终端机，压力传感器设置有两组，分别设置在大臂的关节油缸的进油口与出油口，对关节油缸的进油口与出油口的压力进行监测，角度传感器用于监测驾驶室、底盘、推土铲、大臂、小臂、夹爪的角度变化，距离传感器用于扫描工程作业机器人与地面的接触状态，电液比例阀设置在工程作业机器人的液压油路总阀处，发生危险预警时，对工程作业机器人进行减速或者制动，控制器接收压力传感器、角度传感器、距离传感器数据，传递给终端机，终端机显示数据并对相关数据进行计算，判定倾覆风险，通过控制器，控制电液比例阀，对工程作业机器人进行减速或者制动，安装及使用难度低，能够快速布置于同类型的通用工程作业机器人上，相关参数可以设置并调整变化，灵活度高；终端机采用总线通信协议，便于与其他类型智能设备集成，可采用有线、无线通信方式安装于车身驾驶室或远程控制终端上，操作人员通过终端机的显示器可直观观察风险状况。
14.提供一种工程作业机器人防倾覆预警方法，包括如下步骤：
15.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；
16.s2、获取倾覆支点，距离传感器对工程作业机器人与地面接触的底盘部分进行逐点扫描，定义扫描得到的距离为d，根据距离传感器的坐标以及距离d的长度与角度，得到与地面接触点的坐标，通过与地面接触点的坐标值的情况，判断工程作业机器人的四个支点的位置；四个支点组成四边形，标定相邻的两个支点的x轴坐标与y轴坐标的中间值为倾覆点，倾覆点位于x轴与y轴的平面上，从而确定前、后、左、右四个方向的倾覆点分别为pj，j＝1,2,3,4；
17.s3、获取目标质量，抓取到目标后，控制小臂、夹具垂直朝向地面方向，根据角度传感器、压力传感器的数据综合评估目标质量；
18.s4、倾覆力矩计算，通过各部分组件的质量以及各组件的角度，计算得到各部分组件的质心的坐标，底盘以上部分的质量为mu，质心为cu,底盘及推土铲质量为md，质心为cd,全部质量m为底盘以上部分的质量mu与底盘及推土铲质量为md的和,定义为底盘以上部分与底盘间旋转的相对角度，mu、md、相结合计算得到工程作业机器人整体的质心c，根据计算得到的全部质量m以及质心c计算得到工程作业机器人的前、后方向力矩与左、右方向力矩；
19.s5、倾覆风险评估准则，定义基准力矩为工程作业机器人位于稳定状态时的前、后方向力矩与左、右方向力矩，定义当前时刻力矩为此时状态下的前、后方向力矩与左、右方向力矩，分别计算得到四个倾覆点的基准力矩以及当前时刻力矩，定义同方向的基准力矩
中最小值为倾覆力矩，倾覆点当前时刻力矩与基准力矩的比值为工程作业机器人的倾覆概率；
20.s6、倾覆风险预警阈值定义，根据实时的倾覆概率，可调整预警的阈值，将其分为无、低、中、高、事故几个等级，实现分级预警；
21.s7、基于运动预测的动态倾覆风险评估，根据工程作业机器人当前的操控指令，通过速度、角速度积分方式，预测工程作业机器人的底盘、各组件的运动趋势，预测1、2、4秒等自定义时刻之后的平台姿态，根据预测的平台姿态，计算预测的倾覆力矩，计算预测的倾覆概率，将自定义时刻后的倾覆风险预警结果及当前时刻操作动作指令，发送至终端机的显示屏；
22.s8、高风险作业的自动干预，当工程作业机器人按照当前运动指令运动时，检测到将进入高风险倾覆状态时，提供声光报警。
23.在一种可能的实现方式中，在步骤s2中，以与地面接触点的坐标的z(i)值的变化，来判定支点的位置，考虑地面高度不平，在规定误差范围内，可在小的阈值内认为与地面接触点的坐标的z(i)值为0，z(i)值从负到正或从正到负，则支点位于z(i)值为0处及底盘一端，如果z(i)值大于零，则支点在底盘的两端，如果z(i)值位于中间时大于零，位于两端时小于零，则前侧支点与后侧支点分别位于z(i)值为0处。
24.在一种可能的实现方式中，在步骤s3中，以大臂铰接在底盘的铰接点为支点，根据力矩平衡原则，大臂油缸的力矩等于大臂、小臂、夹爪和目标质量的合力矩，根据几何关系及大臂油缸、大臂、小臂、夹爪的机械参数，求得目标质量。
25.在一种可能的实现方式中，定义组件的质心与该组件的铰接点之间的连线与该组件两个铰接点之间的连线的夹角为固定偏差角，通过固定偏差角以及各组件的长度等，通过几何关系计算，得到合力矩计算所需的相关长度。
26.在一种可能的实现方式中，在步骤s5中，工程作业机器人的稳定状态时，工程作业机器人的各组件保持静态且位于默认收起初态时为稳定状态。
27.在一种可能的实现方式中，在步骤s8中，检测到将进入高风险倾覆状态时，控制器控制电液比例阀从而降低部件的运动速度。
28.在一种可能的实现方式中，在步骤s8中，检测到将进入高风险倾覆状态时，控制器控制电液比例阀对工程作业机器人的运动进行锁定。
29.本发明提供的一种工程作业机器人防倾覆预警装置及方法的有益效果在于：
30.与现有技术相比，考虑并评估工程作业机器人本体的倾覆力矩，还加入了支点变化、目标重量、驾驶员重量、角度变化等变量因素，因此判断结果更加准确，具备预测功能，既能够预判风险又能实现紧急制动，并能判断引发风险的操作指令，进一步提高了机械、人员的安全作业系数和判断准确性。
附图说明
31.为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
32.图1为本发明实施例提供的工程作业机器人的结构示意图；
33.图2为本发明实施例提供的防倾覆预警方法的流程图一；
34.图3为本发明实施例提供的防倾覆预警方法的流程图二。
35.图中：1、底盘；2、推土铲；3、驾驶室；4、大臂；5、小臂；6、夹爪；7、目标；9、压力传感器；10、角度传感器；11、距离传感器；12、电液比例阀；13、控制器；14、关节油缸。
具体实施方式
36.为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
37.请参照图1，现对本发明提供的一种工程作业机器人防倾覆预警装置及方法的一种具体实施方式进行说明，包括压力传感器9、角度传感器10、距离传感器11、电液比例阀12、控制器13及终端机；
38.压力传感器9设置有两组，两组所述压力传感器9分别设置在大臂4的关节油缸14的进油口与出油口，用于监测关节油缸14进油口与出油口处的压力；
39.角度传感器10设置有多个，多个角度传感器10分别设置在工程作业机器人的驾驶室3、底盘1、推土铲2、大臂4、小臂5、夹爪6上，且设置在驾驶室3、底盘1、推土铲2、大臂4、小臂5、夹爪6上远离转动铰接部位的位置，避免组件转动铰接处的相对转动对组件转动角度的影响，用于进行驾驶室3、底盘1、推土铲2、大臂4、小臂5和夹爪6角度的监测，并且标定实际测量的驾驶室3、底盘1、推土铲2、大臂4、小臂5、夹爪6各部件之间角度的差值；
40.距离传感器11设置在工程作业机器人上，可设置在工程作业机器人驾驶室3的顶端，用于扫描工程作业机器人在地面的状态，可对工程作业机器人的底盘1与地面接触的部位进行逐点扫描，得到距离传感器11与底盘1在地面的接触点的距离；
41.电液比例阀12设置在工程作业机器人的液压油路总阀处，控制器13可控制电液比例阀12；
42.控制器13设置在工程作业机器人上，用于接收所述压力传感器9、所述角度传感器10以及所述距离传感器11的数据，将数据传输到终端机，进行分析，并接收终端机反馈的指令信息，一旦具有倾覆的危险，所述控制器13可控制所述电液比例阀12开启或关闭，以对工程作业机器人进行制动；
43.终端机与所述控制器13通过总线协议进行通信，所述终端机可通过设置相关参数，显示各组件的实时状态，终端机上设置有相关软件，所述终端机通过所述控制器13获取所述压力传感器9、所述角度传感器10以及所述距离传感器11相关数据，通过软件计算目标7重量、倾覆力矩并评估倾覆风险，所述终端机可控制所述控制器13降低车速并启动紧急制动。
44.本发明提供的一种工程作业机器人防倾覆预警装置，与现有技术相比，包括压力传感器9、角度传感器10、距离传感器11、电液比例阀12、控制器13及终端机，压力传感器9设置有两组，分别设置在大臂4的关节油缸14的进油口与出油口，对关节油缸14的进油口与出油口的压力进行监测，角度传感器10用于监测驾驶室3、底盘1、推土铲2、大臂4、小臂5、夹爪6的角度变化，距离传感器11用于扫描工程作业机器人与地面的接触状态，电液比例阀12设
置在工程作业机器人的液压油路总阀处，发生危险预警时，对工程作业机器人进行减速或者制动，控制器13接收压力传感器9、角度传感器10、距离传感器11数据，传递给终端机，终端机显示数据并对相关数据进行计算，判定倾覆风险，通过控制器13，控制电液比例阀12，对工程作业机器人进行减速或者制动，安装及使用难度低，能够快速布置于同类型的通用工程作业机器人上，相关参数可以设置并调整变化，灵活度高；终端机采用总线通信协议，便于与其他类型智能设备集成，可采用有线、无线通信方式安装于车身驾驶室3或远程控制终端上，操作人员通过终端机的显示器可直观观察风险状况。
45.作为本发明提供的一种工程作业机器人防倾覆预警方法的一种具体实施方式，请参照图1至图3，包括如下步骤：建立工程作业机器人的坐标系、获取倾覆支点、获取目标质量、倾覆力矩计算、倾覆风险评估准则、倾覆风险预警阈值定义、基于运动预测的动态倾覆风险评估、高风险作业的自动干预。
46.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，其中底盘、推土铲、驾驶室、大臂、小臂、夹爪的质量与质心可由生产厂商提供，目标的质量与质心可计算得到，操作人员的质量与质心可测量得到；
47.s2、获取倾覆支点，距离传感器对工程作业机器人与地面接触的底盘部分进行逐点扫描，定义扫描得到底盘与地面接触点到距离传感器的距离为d，定义距离d与z轴的夹角为θi，距离传感器可设置在驾驶室的顶部或其他易于安装的位置，底盘部分具有一定的宽度，距离传感器在底盘的两侧各安装一组，便于进行底盘两侧与地面接触点的扫描，两组距离传感器对称安装，距离传感器的坐标为(xd,yd,zd)，与地面接触点的坐标为(xi,yi,zi)，距离传感器在竖直平面内转动，距离传感器可读取距离d的长度值以及角度值θi，根据距离传感器的坐标以及距离d的长度d、角度θi，通过三角函数的计算，得到与地面接触点的坐标为(dsin(θi)+xd,
±
yd,-dcos(θi)+zd)，其中dsin(θi)为距离传感器与扫描的地面接触点的x轴方向的长度，扫描的地面接触点的x轴坐标为dsin(θi)与距离传感器的x轴坐标xd之和，距离传感器设置在底盘其中一侧的竖直平面内，距离传感器与扫描的地面接触点的y轴方向的长度与距离传感器的y轴坐标值的数值相等，其中dcos(θi)为距离传感器与扫描的地面接触点的z轴方向的长度，扫描的地面接触点的z轴坐标为距离传感器的z轴坐标zd与dcos(θi)之差，通过与地面接触点的坐标值的情况，判断工程作业机器人的四个支点的位置；以与地面接触点的z轴坐标的z(i)值的变化，来判定支点的位置，与地面接触点的z(i)值的变化随着地面的路况情况而变化，当工程作业机器人在坡面上运动时，与地面接触点的z(i)值则会出现由正到负或者由负到正的变化，当地面不平时工程作业机器人则有可能碾压到砖头等异物，此时距离传感器扫描到的是底盘与砖头等异物的接触点，所得到的z(i)值为正，当工程作业机器人在沙土路面活动时，会将沙土扬起，所得到的z(i)值为扬起的沙土与底盘接触的点，此时得到的z(i)值高出地面的高度，所得到的z(i)值为正，考虑地面高度不平，在规定计算误差范围内，可在小的阈值内认为与地面接触点的坐标的z(i)值为0，z(i)值从负到正或从正到负，在z(i)值为0处，地面对工程作业机器人进行支撑，与此同时地面对底盘的一端进行
支撑，确定z(i)值为0处为支点及底盘一端，如果z(i)值始终大于零，工程作业机器人运行在沙土类地面，底盘下陷，沙土对底盘的两端进行支撑，确定底盘的两端为支点，如果z(i)值位于中间时大于零，位于两端时小于零，在z(i)值为0处，地面对工程作业机器人进行支撑，则前侧支点与后侧支点分别位于z(i)值为0处；可得到工程作业机器人位于左前方、右前方、左后方、右后方的四个支点，根据常规倾覆情况判断，一般认为工程作业机器人向前方、后方、左方、右方倾覆，因此以工程作业机器人的正前方、正后方、正左方、正右方四个方向的倾覆可能性来判断倾覆情况，四个支点组成四边形，标定相邻的两个支点的x轴坐标与y轴坐标的中间值为倾覆点，倾覆点位于x轴与y轴的平面上，从而确定前、后、左、右四个方向的倾覆点分别为pj，j＝1,2,3,4；
48.s3、获取目标质量，抓取到目标后，控制小臂、夹具垂直朝向地面方向，根据角度传感器、压力传感器的数据综合评估目标质量；以大臂铰接在底盘的铰接点为支点，根据力矩平衡原则，大臂油缸的力矩等于大臂、小臂、夹爪和目标质量的合力矩，根据几何关系及大臂油缸、大臂、小臂、夹爪的机械参数，求得目标质量，定义组件的质心与该组件的铰接点之间的连线与该组件两个铰接点之间的连线的夹角为固定偏差角，通过固定偏差角以及各组件的长度等，通过几何关系计算，得到合力矩计算所需的相关长度。
49.s4、倾覆力矩计算，通过各部分组件的质量以及各组件的角度，结合已知的各部件的机械参数，计算得到各部分组件的质心的坐标，底盘以上部分的质量为mu，mu为底盘上方各部件的质量的和，质心为cu,mi/mu为其中一个部件的质量与总质量的比值，质心cu为底盘上方各部件的质心与该比值的乘积之和,底盘及推土铲质量为md，质心为cd，md为底盘与推土铲的质量的和，质心为cd,mi/md为其中一个部件的质量与总质量的比值，质心cd为底盘与推土铲的质心与该比值的乘积之和,全部质量m为底盘以上部分的质量mu与底盘及推土铲质量为md的和,m＝mu+md,定义为底盘以上部分与底盘间旋转的相对角度，m、mu、md、cd、cu相结合利用旋转矩阵计算得到工程作业机器人整体的质心c，根据计算得到的全部质量m以及质心c计算得到工程作业机器人的前、后方向力矩与左、右方向力矩，选取的pj为前方的倾覆点，则计算前方力矩，选取的pj为后方的倾覆点，则计算后方力矩，选取的pj为左方的倾覆点，则计算左方力矩，选取的pj为右方的倾覆点，则计算右方力矩，工程作业机器人的前、后方向力矩计算公式为m
pj
＝mgcosθ(pj(x)-c(x))-mgsinθc(z)，其中θ为工程作业机器人的俯仰角，即平行于工程作业机器人的底盘的轴线并指向工程作业机器人运动前方的向量与水平平面的夹角，前、后方向力矩为全部质量m在x轴方向上力矩之和，借助几何关系以及物理关系，对工程作业机器人的全部重力mg沿着x轴与z轴
进行分解，前、后方向力矩为重力mg在x轴的力矩与z轴之差，重力mg与水平面之间夹角为90
°
，重力mg与xoy平面之间夹角为θ的余角，即重力mg与z轴之间夹角与θ值相等，重力mg作用在x轴的力为mgcosθ，所产生的力臂为倾覆点与质点的x坐标的差值pj(x)-c(x)，重力mg在x轴的力矩为mgcosθ(pj(x)-c(x))，重力mg作用在z轴的力为mgsinθ，所产生的力臂为质点的z坐标c(z)，重力mg在z轴的力矩为mgsinθc(z)，得到前、后方向力矩为工程作业机器人的左、右方向力矩计算公式为其中φ为工程作业机器人的滚转角，即平行于工程作业机器人的底盘的轴线并指向工程作业机器人运动左方的向量与水平平面的夹角，左、右方向力矩为全部质量m在y轴方向上力矩之和，借助几何关系以及物理关系，对工程作业机器人的全部重力mg沿着y轴与z轴进行分解，左、右方向力矩为重力mg在y轴的力矩与z轴之差，重力mg与水平面之间夹角为90
°
，重力mg与xoy平面之间夹角为φ的余角，即重力mg与z轴之间夹角与φ值相等，重力mg作用在y轴的力为mgcosφ，所产生的力臂为倾覆点与质点的y坐标的差值pj(y)-c(y)，重力mg在y轴的力矩为mgcosφ(pj(y)-c(y))，重力mg作用在z轴的力为mgsinφ，所产生的力臂为质点的z坐标c(z)，重力mg在z轴的力矩为mgsinφc(z)，得到前、后方向力矩为
50.s5、倾覆风险评估准则，定义基准力矩为工程作业机器人位于稳定状态时的四个倾覆点的前、后方向力矩或左、右方向力矩，定义当前时刻力矩为此时状态下的四个倾覆点的前、后方向力矩或左、右方向力矩，分别计算得到四个倾覆点的基准力矩以及当前时刻力矩,定义同方向的基准力矩中，最小值为该方向的倾覆力矩，倾覆点当前时刻力矩与基准力矩最小值的比值为工程作业机器人的倾覆概率工程作业机器人的稳定状态时，工程作业机器人的各组件保持静态且位于默认收起初态时为稳定状态；
51.s6、倾覆风险预警阈值定义，根据实时的倾覆概率，可调整预警的阈值，将其分为无、低、中、高、事故几个等级，实现分级预警；
52.s7、基于运动预测的动态倾覆风险评估，根据工程作业机器人当前的操控指令，通过速度、角速度积分方式，预测工程作业机器人的底盘、各组件的运动趋势，预测1、2、4秒等自定义时刻之后的平台姿态，根据预测的平台姿态，计算预测的倾覆力矩，计算预测的倾覆概率，将自定义时刻后的倾覆风险预警结果及当前时刻操作动作指令，发送至终端机的显示屏；
53.s8、高风险作业的自动干预，当工程作业机器人按照当前运动指令运动时，检测到将进入高风险倾覆状态时，提供声光报警；在步骤s8中，检测到将进入高风险倾覆状态时，控制器控制电液比例阀从而降低部件的运动速度；检测到将进入高风险倾覆状态时，控制器控制电液比例阀对工程作业机器人的运动进行锁定。
54.本发明提供的一种工程作业机器人防倾覆预警方法，与现有技术相比，考虑并评
估工程作业机器人本体的倾覆力矩，还加入了支点变化、目标重量、驾驶员重量、角度变化等变量因素，因此判断结果更加准确，具备预测功能，既能够预判风险又能实现紧急制动，并能判断引发风险的操作指令，进一步提高了机械、人员的安全作业系数和判断准确性。
55.以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。