本发明属于工程机械领域,涉及一种可自主规划轨迹的挖掘机臂智能控制系统及方法。
背景技术:
作为常见的工程机械装备,挖掘机在工程施工领域的作用不容小觑。但是,传统挖掘机设备在使用过程中,施工环境与疲劳操作所引发的不确定性,轻则降低操作者的施工效率,重则引发安全事故。
挖掘机臂——它是挖掘机的作业装置,操作者通过对它的手动操作可以完成相应的施工任务,它包含中心回转体及其所连接的上车体、动臂、斗杆、铲斗以及连接各个部件的运动副,在控制领域,可以将其等效为一个4r机械臂来加以控制。
在挖掘机臂的路径规划与运动控制方面,目前主要存在两种实现方案:
其一,传统方法,即依据挖掘机臂的运动学模型将已知的铲斗末端运动轨迹转换为挖掘机关节空间中的运动轨迹,从而实现从挖掘点到卸载点的轨迹规划及控制。
其二,基于人工智能技术的实现方法,基于人工智能算法,运用熟练操作者的操作数据建立适用于挖掘机的智能控制模型,并将其应用于真实的挖掘机中,从而实现挖掘机臂的轨迹规划及控制。
对现有技术而言,不论是传统方法,还是基于人工智能的方法,都有一定的局限性。
(1)适应性差:首先,从时间维度来看,铲斗末端的可行(乃至最佳)运动轨迹会随物料形态的变化而变化;其次,从空间维度来看,铲斗末端的可行(乃至最佳)运动轨迹会随施工场景的变化而变化。而不论是传统方法还是基于人工智能的方法,都不能很好地适应这两方面的变化。
(2)开发成本高:首先,在运用人工智能方法开发挖掘机臂轨迹规划算法的过程中,需要在开发调试工作之前获取大量的操作人员的施工数据,从而消耗大量的人力物力;其次,挖掘机内部相对运动规律的复杂性决定了挖掘机臂动力学模型的复杂性,因此,在运用传统方法开发挖掘机臂轨迹规划算法的过程中,有可能需要同时对多个控制参数进行调试,从而使开发过程变得复杂,进而延长开发周期。
技术实现要素:
本发明针对现有技术的不足,提供一种可自主规划轨迹的挖掘机臂智能控制系统及方法,在施工过程中,该系统可自行采集施工环境数据并据其完成挖掘机臂的运动规划,最终对挖掘机臂的运动实现最优化控制,借以提高传统设备的工作效率并保证操作者的人身安全。
为了实现上述发明目的,本发明采用以下技术方案:一种可自主规划轨迹的挖掘机臂智能控制系统,其特征在于,所述的控制系统由环境感知模块、轨迹规划模块以及执行模块组成;所述的环境感知模块,用于在施工过程中采集挖掘机臂的施工环境数据;所述的轨迹规划模块,利用环境感知模块采集的施工环境数据完成挖掘机臂的运动规划;所述的执行模块,依据轨迹规划模块的输出结果完成挖掘机臂的运动控制;
轨迹规划模块包含状态决策子模块、模型预测子模块以及动作选择子模块;
所述状态决策子模块,其利用施工环境数据针对当前的施工环境状态做出决策,即得到环境状态s和反馈值r;
所述模型预测子模块,依据当前计算的反馈值r,它可以将状态决策子模块得到的环境状态s转化为q值,q值用于量度挖掘机臂智能控制系统工作过程中机械臂在各个自由度上不同动作的发生概率;
所述的动作选择子模块,其依据来自状态决策子模块的反馈值r与来自模型预测子模块的q值完成运动规划。
状态决策子模块包含数据融合单元与反馈值计算单元;所述数据融合单元,用于将来自环境感知模块的数据组成有效的数据形式,即环境状态s;所述反馈值计算单元,依据环境状态s给出一个反馈值r;环境状态s包含利用施工环境数据推算得到的障碍物中心点信息、在不同方向上的障碍物尺度信息以及挖掘机臂到障碍物的距离信息;
在
假设t0时刻挖掘机臂上的点构成一个集合s,那么
首先,计算方向
其次,计算
最后,计算
其中,
反馈值r的取值应满足以下三个原则:可自主规划轨迹的挖掘机臂智能控制系统工作过程中,挖掘机臂到障碍物objk的距离lk应该不断增大,即
首先,求取挖掘机臂关节运动的期望速度
其中,vf_maxi为挖掘机臂在自由度i上正向运动的最大速度,而vb_maxi为挖掘机臂在自由度i上反向运动的最大速度,
其次,对求得的期望速度
其中,pre_ri为将求得的自由度为i时的期望速度
最后,计算反馈值r:
r=(r·i)
其中,ki为增益系数,r·i为挖掘机臂在自由度i上的各个反馈值所构成的列向量,而i=1,2,…,n。
所述的动作选择单元包含价值更新单元与运动规划单元;所述价值更新单元,根据反馈值r对来自深度模型的q值进行更新;所述运动规划单元,依据价值更新单元得到的更新后的q值完成运动规划;
价值更新单元的运算公式为:
q(s1|r)=λ1r+λ2q(s0|r)
q=(q″ji)
其中j=1,2,…,m,i=1,2,…,n;
其中,λ1为权重系数,λ2为衰减系数,s0为一个控制周期以前的环境状态,s1为当前的环境状态;q″ji表示在自由度i上发生的动作j的概率的量度;
运动规划单元的运算公式为:
对q=(q·i)而言,运动规划的过程分为两个步骤:
其一,挑选q·i中的最大元素pi,即
pi=max(q·i)×flagωi(ω∈{η|qηi=max(q·i)});
其二,依据如下公式计算自由度i的运动速度vi,
vi=κipi+ksi
其中,κi为针对自由度i的增益系数,ksi为针对自由度i的前馈速度值,q·i表示挖掘机臂在自由度i上各种动作发生概率,而i=1,2,…,n;{η|qηi=max(q·i)}表示在自由度i上有可能发生全体动作的序数所构成集合的一个子集,其代表元素为η,此时挖掘机臂在自由度i上发生该子集元素所对应动作的概率最大,而ω正是这个子集的某个元素;因此,flagωi表示在自由度i上发生概率最大的动作所对应的速度标志位。
所述的动作选择单元根据q值矩阵中元素取值大小来确定各个运动部件当前的运动方向、运动速度;运动部件包括中心回转体、动臂、斗杆与铲斗;运动方向为正向运动、不运动、反向运动。
所述执行模块包括挖掘机臂本体结构中的液压系统以及电气控制系统。
本发明还提供一种可自主规划轨迹的挖掘机臂智能控制方法,包括如下步骤:
1)采用环境感知模块在施工过程中采集挖掘机臂的施工环境数据;
2)通过轨迹规划模块针对施工环境数据进行处理以完成挖掘机臂的运动规划;
3)执行模块依据轨迹规划模块的输出结果完成挖掘机臂的运动控制;
步骤2)具体为:轨迹规划模块中状态决策子模块,利用施工环境数据针对当前的施工环境状态做出决策,即得到环境状态s和反馈值r;再采用轨迹规划模块中的模型预测子模块,依据当前计算的反馈值r,将状态决策子模块得到的环境状态s转化为q值,q值用于量度挖掘机臂智能控制系统工作过程中机械臂在各个自由度上不同动作的发生概率;轨迹规划模块中的动作选择子模块依据来自状态决策子模块的反馈值r与来自模型预测子模块的q值完成运动规划。
一种可自主规划轨迹的挖掘机臂智能控制方法,具体包括如下步骤:
1.完成环境感知模块、轨迹规划模块以及执行模块的初始化;
2.环境感知模块采集施工环境数据;
3.数据融合单元读取来自环境感知模块的施工环境数据;
4.数据融合单元将施工环境数据转化为环境状态s,算法如下,
5.反馈值计算单元读取来自数据融合单元的环境状态s;
6.反馈值计算单元依据环境状态s计算反馈值r,算法如下,
r=(r·i);
7.利用反馈值r完成对模型预测子模块的修正;
8.模型预测子模块读取来自数据融合单元的环境状态s;
9.在当前反馈值r的条件下,模型预测子模块依据环境状态s计算q值,具体为q=q(s|r);
10.价值更新单元读取来自模型预测子模块的q值;
11.价值更新单元读取来自反馈值计算单元的反馈值r;
12.价值更新单元利用最新的反馈值r更新q值,算法如下,
q(s1|r)=λ1r+λ2q(s0|r)
q=(q″ji)
13.运动规划单元读取来自价值更新单元的q值;
14.运动规划单元利用更新后的q值完成运动规划,算法如下,
pi=max(q·i)×flagωi(ω∈{η|qηi=max(q·i)})
vi=κipi+ksi;
15.运动规划单元将运动规划结果发送给执行模块;
16.执行模块实现挖掘机臂的运动控制;
17.判断施工过程是否结束,若是,停止对挖掘机臂的运动控制,否则,返回第2步。
相对于现有技术,本发明的有益效果为:在施工过程中,用户只要开启本发明所公开的智能控制系统,该系统可自行采集施工环境数据并据其完成挖掘机臂的运动规划,最终对挖掘机臂的运动实现最优化控制,从而将劳动者从危险且繁重的劳动中解放出来,增大了施工过程中的安全系数,同时可大大提高施工效率。
附图说明
图1为本发明挖掘机臂智能控制系统的实现框图;
图2为本发明的一个实施样例中轨迹规划模块的实现框图;
图3为本发明的一个实施样例中轨迹规划模块之状态决策子模块的实现框图;
图4为本发明的一个实施样例中轨迹规划模块之动作选择子模块的实现框图;
图5为本发明的一个实施样例中挖掘机臂智能控制系统的实现框图;
图6为本发明的一个实施样例中挖掘机臂智能控制系统的工作流程图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步详细介绍。
本发明公开了一种可自主规划轨迹的挖掘机臂智能控制系统及方法,在施工过程中,该系统可自行采集施工环境数据并据其完成挖掘机臂的运动规划,最终对挖掘机臂的运动实现最优化控制。
该控制系统由环境感知模块、轨迹规划模块以及执行模块三部分组成,其内涵如下。
所述环境感知模块,用于在施工过程中采集挖掘机臂的施工环境数据。
在一个实施样例中,环境感知模块可包含导航单元、雷达单元以及图像视频单元等硬件设备以及适用于这些硬件设备的数据采集单元。具体地,这些施工环境数据可以包含挖掘机臂工作空间中的障碍物列表obj_set以及三维随机向量
所述轨迹规划模块,它可以利用施工环境数据完成挖掘机臂的运动规划,它是整个控制系统的核心成分。在一个实施样例中,轨迹规划模块可包含状态决策子模块、模型预测子模块以及动作选择子模块。
所述状态决策子模块,它可以利用施工环境数据针对当前的施工环境状态做出决策,在一个实施样例中,状态决策单元可包含数据融合单元与反馈值计算单元。
所述数据融合单元,它可以将来自环境感知模块的数据组成有效的数据形式,不妨称其为环境状态s。具体地,这里的环境状态s可以包含利用施工环境数据推算得到的障碍物中心点信息、在不同方向上的障碍物尺度信息以及挖掘机臂到障碍物的距离信息。
事实上,对障碍物obj∈obj_set而言,其中心点应当是这样一个点
求得障碍物中心点信息后,可以在
假设t0时刻挖掘机臂上的点构成一个集合s,那么
首先,计算方向
其次,计算
最后,计算
所述反馈值计算单元,它可以依据环境状态数据给出一个反馈值r,具体地,反馈值r是一个大小为m×n的矩阵,其中,m在数值上等于机械臂在特定自由度上有可能发生的动作数目;而n在数值上等于机械臂的自由度数目,m、n均为自然数。相应地,对挖掘机臂而言,它包含中心回转体、动臂、斗杆与铲斗四个运动部件,而每个运动部件都有正向运动、不运动以及反向运动三种动作,因此,在一个实施样例中,反馈值r是一个3×4大小的矩阵。事实上,反馈值r所要量度的是该系统工作过程中机械臂在各个自由度上不同动作发生概率的补偿值。
因此,在一个实施样例中,反馈值r的取值应满足以下三个原则:首先,该系统工作过程中,挖掘机臂到障碍物objk距离lk应该不断增大,即
首先,求取挖掘机臂关节运动的期望速度
其中,vf_maxi为挖掘机臂在自由度i上正向运动的最大速度,而vb_maxi为挖掘机臂在自由度i上反向运动的最大速度,
其次,对求得的期望速度
其中,pre_ri为将求得的自由度为i时的期望速度
最后,计算反馈值r:
r=(r·i)
其中,ki为增益系数,r·i为挖掘机臂在自由度i上的各个反馈值所构成的列向量,而i=1,2,…,n。
所述模型预测子模块,依据当前计算的反馈值r,它可以将当前的环境状态s转化为一个q值。事实上,q值所量度的是该系统工作过程中机械臂在各个自由度上不同动作的发生概率,控制系统可以依据q值完成对机械臂的运动规划。如前文所述,对挖掘机臂而言,反馈值r是一个3×4大小的矩阵。此外,该系统在工作过程中可以利用来自反馈值计算单元的反馈值r完成对所述模型预测子模块的修正。
在一个实施样例中,所述模型预测子模块可以用深度神经网络来实现。据前文所述,模型预测子模块的功能可用如下公式表示:q=q(s|r)。
所述动作选择子模块,它可以依据来自状态决策子模块的反馈值r与来自模型预测子模块的q值完成运动规划,在一个实施样例中,动作选择子模块可包含价值更新单元与运动规划单元。
所述价值更新单元,它可以根据反馈值r对来自模型预测子模块的q值进行更新,在一个实施样例中,这个更新机制可以基于bellman方程来构建,具体如下。
q(s1|r)=λ1r+λ2q(s0|r)(1)
q=(q″ji)(4)
j=1,2,…,m,i=1,2,…,n
其中,λ1为权重系数,λ2为衰减系数,s0为一个控制周期以前的环境状态,s1为当前的环境状态。q″ji表示在自由度i上发生的动作j的概率的量度。其中,公式(1)代表依据bellman方程对当前的q值进行更新,由于文中已经说明,q值(其元素)是对概率的量度,而根据柯尔莫哥洛夫对概率的公理化定义(非负性、归一性与可列可加性),bellman方程的计算结果是不一定满足以上三个条件的,所以运用公式(2)与公式(3)对q值的元素进行修正,其中,公式(2)保证其满足非负性,公式(3)保证其满足归一性。q′ji为对qji第一次修正后的结果;q″ji为对qji第二次修正后的结果。
所述运动规划单元,它可以依据更新后的q值完成运动规划。事实上,针对机械臂的每个自由度i可以定义速度标志位flagji,而j=1,2,…,m,对挖掘机臂而言,其每个运动部件都有正向运动、不运动以及反向运动三种动作,故j=1,2,3,因此,有如下定义,
基于此,对q=(q·i)而言,运动规划的过程可分为两个步骤:
其一,挑选q·i中的最大元素pi,即
pi=max(q·i)×flagωi(ω∈{η|qηi=max(q·i)});
其二,依据如下公式计算自由度i的运动速度vi,
vi=κipi+ksi
其中,κi为针对自由度i的增益系数,ksi为针对自由度i的前馈速度值,q·i表示挖掘机臂在自由度i上各种动作发生概率,而i=1,2,…,n;{η|qηi=max(q·i)}表示在自由度i上有可能发生全体动作的序数所构成集合的一个子集,其代表元素为η,此时挖掘机臂在自由度i上发生该子集元素所对应动作的概率最大,而ω正是这个子集的某个元素;因此,flagωi表示在自由度i上发生概率最大的动作所对应的速度标志位。其中,在某个自由度上可以发生三个动作(正向运动、不运动与反向运动),这三种动作构成的全体称为全体动作。
所述执行模块,它可以依据轨迹规划模块的输出结果完成挖掘机臂的运动控制。在一个实施样例中,该模块可以包含挖掘机臂本体结构中的液压系统以及电气控制系统。
如图1所示,本发明主要包括环境感知模块、轨迹规划模块与执行模块。
所述环境感知模块,用于在施工过程中采集挖掘机臂的施工环境数据。
所述轨迹规划模块,它可以利用施工环境数据完成挖掘机臂的运动规划,它是整个控制系统的核心成分。
所述执行模块,它可以依据轨迹规划模块的输出结果完成挖掘机臂的运动控制。
如图2所示,在本发明的一个实施样例中,轨迹规划模块中可以包括状态决策子模块、模型预测子模块与动作选择子模块。
所述状态决策子模块,它可以利用施工环境数据针对当前的施工环境状态做出决策。
所述模型预测子模块,依据当前计算的反馈值r,它可以将当前的环境状态s转化为一个q值。
所述动作选择子模块,它可以依据来自状态决策子模块的反馈值r与来自模型预测子模块的q值完成运动规划。
图中序号1代表该系统利用来自状态决策子模块的最新反馈值r修正模型预测子模块的控制流传输过程。
图中序号2代表模型预测子模块读取来自状态决策子模块的反馈值r的数据流传输过程。
图中序号3代表动作选择子模块读取来自模型预测子模块的q值的数据流传输过程。
图中序号4代表动作选择子模块读取来自状态决策子模块的反馈值r的数据流传输过程。
如图3所示,在本发明的一个实施样例中,轨迹规划模块中的状态决策子模块可以包括数据融合单元与反馈值计算单元。
所述数据融合单元,它可以将来自环境感知模块的数据组成有效的数据形式,不妨称其为环境状态s,算法如下:
环境状态s所涉及的是障碍物的特征数据。本文中没有带上(下)标k的地方则是对障碍物特征数据的提取算法展开论述。而带有上(下)标k的地方则是运用特征提取算法按照序数k提取障碍物列表中各个障碍物的数据特征的过程。
所述反馈值计算单元,它可以依据环境状态数据给出一个反馈值r,算法如下:
r=(r·i);
图中序号1所代表的数据流传输过程发生在状态决策子模块中,具体为其中的反馈值计算单元读取来自数据融合单元的环境状态s。
如图4所示,在本发明的一个实施样例中,轨迹规划模块中的动作选择子模块可以包括价值更新单元与运动规划单元。
所述价值更新单元,它可以根据反馈值r对来自模型预测子模块的q值进行更新,算法如下:
q(s1|r)=λ1r+λ2q(s0|r)
q=(q″ji)
所述运动规划单元,它可以依据更新后的q值完成运动规划,算法如下:
pi=max(q·i)×flagωi(ω∈{η|qηi=max(q·i)})
vi=κipi+ksi。
图中序号1所代表的数据流传输过程发生在动作选择子模块中,具体为其中的运动规划单元读取来自价值更新单元的更新后的q值。
如图5所示,在本发明的一个实施样例中,本发明可以包括环境感知模块、数据融合单元、反馈值计算单元、模型预测子模块、价值更新单元、运动规划单元与执行模块。
如上文所述,所述数据融合单元与所述反馈值计算单元共同构成状态决策子模块,所述价值更新单元与所述运动规划单元共同构成动作选择子模块,状态决策子模块、动作选择子模块连同所述模型预测子模块共同构成轨迹规划模块:因此,该实施样例亦可抽象为环境感知模块、轨迹规划模块与执行模块三个组成部分。
图中序号1代表所述环境感知模块采集施工环境数据的数据流传输过程。
图中序号2代表所述数据融合单元读取来自所述环境感知模块的施工环境数据的数据流传输过程。
图中序号3所代表的数据流传输过程发生在状态决策子模块中,具体为所述反馈值计算单元读取来自所述数据融合单元的环境状态s。
图中序号4代表该系统利用来自所述反馈值计算单元的最新反馈值r修正所述模型预测子模块的控制流传输过程。
图中序号5代表所述模型预测子模块读取来自所述数据融合单元的环境状态s的数据流传输过程。
图中序号6代表所述价值更新单元读取来自所述模型预测子模块的q值的数据流传输过程。
图中序号7代表所述价值更新单元读取来自所述反馈值计算单元的反馈值r的数据流传输过程。
图中序号8所代表的数据流传输过程发生在动作选择子模块中,具体为所述运动规划单元读取来自所述价值更新单元的更新后的q值。
图中序号9代表所述运动规划单元将挖掘机臂的运动规划结果发送给所述执行模块的数据流传输过程。
如图5、图6所示,在本发明的一个实施样例中,该智能控制系统可依照如下流程实现挖掘机臂的运动控制:
1.完成各模块的初始化;
2.执行图5中序号1所代表的数据流传输过程(所述环境感知模块采集施工环境数据);
3.执行图5中序号2所代表的数据流传输过程(所述数据融合单元读取来自所述环境感知模块的施工环境数据);
4.所述数据融合单元将施工环境数据转化为环境状态s,算法如下,
5.执行图5中序号3所代表的数据流传输过程(所述反馈值计算单元读取来自所述数据融合单元的环境状态s);
6.所述反馈值计算单元依据环境状态s计算反馈值r,算法如下,
r=(r·i);
7.执行图5中序号4所代表的控制流传输过程(该系统利用来自所述反馈值计算单元的最新反馈值r修正所述模型预测子模块;
8.执行图5中序号5所代表的数据流传输过程(所述模型预测子模块读取来自所述数据融合单元的环境状态s);
9.在当前反馈值r的条件下,模型预测子模块依据环境状态s计算q值,具体为q=q(s|r);
10.执行图5中序号6所代表的数据流传输过程(所述价值更新单元读取来自所述模型预测子模块的q值);
11.执行图5中序号7所代表的数据流传输过程(所述价值更新单元读取来自所述反馈值计算单元的反馈值r);
12.价值更新单元利用最新的反馈值r更新q值,算法如下,
q(s1|r)=λ1r+λ2q(s0|r)
q=(q″ji)
13.执行图5中序号8所代表的数据流传输过程(所述运动规划单元读取来自所述价值更新单元的更新后的q值);
14.运动规划单元利用更新后的q值完成运动规划,算法如下,
pi=max(q·i)×flagωi(ω∈{η|qηi=max(q·i)})
vi=κipi+ksi;
15.执行图5中序号9所代表的数据流传输过程(所述运动规划单元将挖掘机臂的运动规划结果发送给所述执行模块);
16.执行模块实现挖掘机臂的运动控制;
17.判断施工过程是否结束,若是,停止对挖掘机臂的运动控制,否则,返回第2步。
如文中所述,r与q所表示的对象是矩阵。所以,r·i、q·i、qji三个符号的含义分别为r矩阵的第i列的元素(当然,结合反馈值的含义,它同样表示挖掘机臂在自由度i上的各个反馈值所构成的列向量)、q矩阵的第i列的元素(当然,结合q值的含义,它同样表示挖掘机臂在自由度i上各种动作发生概率的量度)、q矩阵的第j行,第i列的元素(当然,结合q值的含义,它同样表示挖掘机臂在自由度i上动作j发生概率的量度)。
需要说明的是,以上所述仅为本发明的实施例,并非因此限制本发明的专利范围,凡是利用本发明说明书内容所做的等效结构或等效流程变换,或直接或间接运用在其他相关技术领域,均同理包括在本发明的专利保护范围内。