一种无人抓钢机运动转向控制方法及系统与流程

1.本发明涉及智能钢铁生产应用领域，尤其涉及一种无人抓钢机运动转向控制方法及系统。


背景技术：

2.在钢铁厂废钢作业过程中，抓钢机通常用于对废钢等材料进行抓取和装载等工作，自动化程度较低，工作效率较低，人员招工困难，容易疲劳驾驶，存在安全隐患。如何提高抓钢机工作效率，实现抓钢机自主运行成为当前一大难题。


技术实现要素：

3.鉴于以上现有技术存在的问题，本发明提出一种无人抓钢机运动转向控制方法及系统，主要解决现有废钢作业自动化程度低影响工作效率的问题。
4.为了实现上述目的及其他目的，本发明采用的技术方案如下。
5.本技术提供一种无人抓钢机运动转向控制方法，包括：
6.获取抓钢机实时定位信息，根据所述实时定位信息确定当前时间节点所述抓钢机的位置坐标，其中所述抓钢机为履带式抓钢机，包括左履带和右履带；
7.获取当前时间节点所述抓钢机的左履带和右履带的瞬时线速度，根据所述瞬时线速度、所述位置坐标建立抓钢机运动学模型，以根据所述抓钢机运动学模型确定所述抓钢机在下一时间节点的位姿状态；
8.根据所述抓钢机初始位置和目标位置进行运动轨迹规划，得到一组不同时间节点的运动轨迹点序列，作为轨迹参考点序列；
9.根据所述下一时间节点的位姿状态与所述轨迹参考点序列中对应轨迹参考点的误差构建抓钢机控制模型，以根据所述抓钢机控制模型控制所述左履带和右履带进行转向使得所述抓钢机向所述目标位置移动。
10.在本技术一实施例中，获取抓钢机实时定位信息，根据所述实时定位信息确定当前时间节点所述抓钢机的位置坐标，包括：
11.根据全球定位系统获取所述抓钢机的定位数据；
12.对所述定位数据进行预处理后转换到地面坐标系，得到所述位置坐标，其中，所述预处理包括滤波校验。
13.在本技术一实施例中，获取当前时间节点所述抓钢机的左履带和右履带的瞬时线速度，根据所述瞬时线速度、所述位置坐标以及预设的抓钢机运动学模型，确定所述抓钢机在下一时间节点的位姿状态，包括：
14.根据所述左履带和所述右履带的瞬时线速度确定所述抓钢机的质心瞬时线速度和瞬时角速度；
15.将所述质心瞬时线速度、质心瞬时角速度以及所述位置坐标建立所述抓钢机运动学模型，得到所述抓钢机下一时间节点的位姿状态，其中所述抓钢机运动学模型包含计算
当前时间节点与下一时间节点的位姿状态关系的状态方程。
16.在本技术一实施例中，根据所述当前时间节点所述抓钢机的位置坐标以及以及所述质心瞬时线速度，计算得到当前时间节点相对于所述目标位置的航向角，根据所述位置坐标和所述航向角确定下一时间节点的所述状态方程，其中所述状态方程表示为：
17.x(t+1)＝x(t)+vccosθ*t
18.y(t+1)＝y(t)+vcsinθ*t
19.θ(t+1)＝θ(t)+ωc*t
20.其中，(x
t
，y
t
)为抓钢机t时刻的位置坐标，θ
t
抓钢机t时刻相对于目标位置的航向角。
21.在本技术一实施例中，根据所述下一时间节点的位姿状态与对应轨迹参考点的误差构建抓钢机控制模型，以根据所述抓钢机控制模型控制所述左履带进行转向，包括：
22.通过所述抓钢机控制模型向所述左履带下发第一实时控制信号，所述第一实时控制信号表示为：
[0023][0024]
其中，α
(
·
)
，β
(
·
)
，γ
(
·
)
，δ
(
·
)
分别表示权重系数，k
p(
·
)
，k
i(
·
)
，k
d(
·
)
分别表示可调控制参数，下标中的l，r分别表示左履带和右履带，下标v，x，y，θ分别表示履带速度和质心位姿。
[0025]
在本技术一实施例中，根据所述下一时间节点的位姿状态与对应轨迹参考点的误差构建抓钢机控制模型，以根据所述抓钢机控制模型控制所述右履带进行转向，包括：
[0026]
通过所述抓钢机控制模型向所述右履带下发第二实时控制信号，所述第二实时控制信号表示为：
[0027][0028]
其中下标中的l，r分别表示左履带和右履带，下标v，x，y，θ分别表示履带速度和质心位姿，k
p(
·
)
，k
i(
·
)
，k
d(
·
)
分别表示可调控制参数，α
(
·
)
，β
(
·
)
，γ
(
·
)
，δ
(
·
)
分别表示权重系数。
[0029]
本技术还提供一种无人抓钢机运动转向控制系统，包括：
[0030]
定位模块，用于获取抓钢机实时定位信息，根据所述实时定位信息确定当前时间节点所述抓钢机的位置坐标，其中所述抓钢机为履带式抓钢机，包括左履带和右履带；
[0031]
位姿预测模块，用于获取当前时间节点所述抓钢机的左履带和右履带的瞬时线速度，根据所述瞬时线速度、所述位置坐标建立抓钢机运动学模型，以根据所述抓钢机运动学模型确定所述抓钢机在下一时间节点的位姿状态；
[0032]
运动规划模块，用于根据所述抓钢机初始位置和目标位置进行运动轨迹规划，得到一组不同时间节点的运动轨迹点序列，作为轨迹参考点序列；
[0033]
转向控制模块，用于根据所述下一时间节点的位姿状态与所述轨迹参考点序列中对应轨迹参考点的误差构建抓钢机控制模型，以根据所述抓钢机控制模型控制所述左履带和右履带进行转向以使所述抓钢机向所述目标位置移动。
[0034]
如上所述，本技术一种无人抓钢机运动转向控制方法及系统，具有以下有益效果。
[0035]
本技术针对抓钢机履带运动的特点，提出了抓钢机运动通过辅助系统获得当前车载状态信息和即将行驶的道路对应的目标道路信息；根据当前车载状态信息和目标道路信息确定无人抓钢机位置以及姿态，建立无人抓钢机控制模型，通过轨迹跟踪控制使得无人抓钢机在一定时间内跟踪到期望轨迹。根据抓钢机左右两侧传动轮拥有独立的动力输出特性，通过获取传感器数据进行分析预测抓钢机转向扭矩信息；若根据预测转向扭矩信息确定初始转向需求不满足目标转向需求，则对无人抓钢机转向扭矩分配进行修正，得到无人抓钢机的最终转向扭矩信息，根据最终转向扭矩信息确定的最终转向需求满足目标转向需求。从而使得抓钢机运动到指定位置，本技术可实现自动运动转向控制，不需要人工干预，提高作业效率。
附图说明
[0036]
图1为本发明一实施例中无人抓钢机运动转向控制方法的流程示意图。
[0037]
图2为本技术一实施例中抓钢机运动模型的示意图。
[0038]
图3为本技术一实施例中无人抓钢机运动转向控制系统的模块图。
具体实施方式
[0039]
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
[0040]
需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
[0041]
请参阅图1，本技术提供一种无人抓钢机运动转向控制方法，该方法包括以下步骤：
[0042]
步骤s01，获取抓钢机实时定位信息，根据所述实时定位信息确定当前时间节点所述抓钢机的位置坐标，其中所述抓钢机为履带式抓钢机，包括左履带和右履带。
[0043]
在一实施例中，获取抓钢机实时定位信息，根据所述实时定位信息确定当前时间节点所述抓钢机的位置坐标，包括：
[0044]
根据全球定位系统获取所述抓钢机的定位数据；
[0045]
对所述定位数据进行预处理后转换到地面坐标系，得到所述位置信息，其中，所述预处理包括滤波校验。
[0046]
具体地，通过gps传感器获取gps定位数据信息，由于gps定位数据不一定准确，因此对gps原始的经纬度数据进行数据校验，校验方法卡尔曼滤波，对数据进行处理。采用米勒投影转换方法地球经纬度转换平面坐标，从而求出x坐标与y坐标。
[0047]
步骤s02，获取当前时间节点所述抓钢机的左履带和右履带的瞬时线速度，根据所述瞬时线速度、所述位置坐标以及预设的抓钢机运动学模型，确定所述抓钢机在下一时间节点的位姿状态。
[0048]
在一实施例中，获取当前时间节点所述抓钢机的左履带和右履带的瞬时线速度，根据所述瞬时线速度、所述位置坐标建立抓钢机运动学模型，以根据所述抓钢机运动学模型确定所述抓钢机在下一时间节点的位姿状态，包括：
[0049]
根据所述左履带和所述右履带的瞬时线速度确定所述抓钢机的质心瞬时线速度和瞬时角速度；
[0050]
将所述质心瞬时线速度、质心瞬时角速度以及所述位置坐标建立所述抓钢机运动学模型，得到所述抓钢机下一时间节点的位姿状态，其中所述抓钢机运动学模型包含计算当前时间节点与下一时间节点的位姿状态关系的状态方程。
[0051]
根据所述当前时间节点所述抓钢机的位置坐标以及以及所述质心瞬时线速度，计算得到当前时间节点相对于所述目标位置的航向角，根据所述位置信息和所述航向角确定下一时间节点的所述状态方程状态方程表示为：
[0052]
x(t+1)＝x(t)+vccosθ*t
[0053]
y(t+1)＝y(t)+vcsinθ*t
[0054]
θ(t+1)＝θ(t)+ωc*t
[0055]
其中，(x
t
，y
t
)为抓钢机t时刻的位置坐标，θ
t
抓钢机t时刻相对于目标位置的航向角。
[0056]
请参阅图2，图2为本技术一实施例中抓钢机运动模型的示意图。根据抓钢机移动底盘特性，构建出抓钢机模型，v
l
，vr分别为抓钢机左履带与右履带瞬时下线速度，vc，ω分别为机器人质心瞬时线速度与角速度，d为左右履带的轮间距。
[0057]
通过安装在左右履带的传感器测量出左右履带的瞬时v
l
，vr的线速度，抓钢机瞬时线速度为抓钢机的瞬时角速度ω为所以可以求得抓钢机的转弯半径
[0058]
以t为采样时间，对上述式子离散化处理，建立状态方程。
[0059]
x(t+1)＝x(t)+vccosθ*t
[0060]
y(t+1)＝y(t)+vcsinθ*t
[0061]
θ(t+1)＝θ(t)+ωc*t
[0062]
因此抓钢机位姿定义为t时刻位姿p
t
＝[x
t
，y
t
，θ
t
]
t
，(x
t
，y
t
)为抓钢机的位置坐标，θ
t
航向角。
[0063]
步骤s03，根据所述抓钢机初始位置和目标位置进行运动轨迹规划，得到一组不同时间节点的运动轨迹点序列，作为轨迹参考点序列。
[0064]
在一实施例中，根据上述抓钢机运动学模型，在已知任务初始位姿信息和目标位姿信息的基础上，通过a*算法对移动底盘的运动轨迹进行规划，获取运动轨迹序列q(n)，其中n＝it，i＝0，1，2，...，算法中代价函数选取为
[0065]
f(n)＝g(n)+h(n)
[0066]
其中g(n)为初始位置与目标位置之间的实际距离，h(n)为初始位置与目标位置之间的曼哈顿距离。
[0067]
依据得到的规划路径，分析自主规划路径的可实现性，根据规划路径的平滑性和履带非完整约束的可行性程度，自主选择采用多次样条曲线或贝塞尔曲线对规划路径进行平滑过渡。
[0068]
s04，根据所述下一时间节点的位姿状态与所述轨迹参考点序列中对应轨迹参考点的误差构建抓钢机控制模型，以根据所述抓钢机控制模型控制所述左履带和右履带进行转向使得所述抓钢机向所述目标位置移动。
[0069]
在一实施例中，上述抓钢机的数学建模和规划曲线，通过抓钢机的实时位姿和速度控制抓钢机运动，换言之通过分别控制左右轮的方式控制机器人向目标点运动。
[0070]
在本技术一实施例中，所述状态方程表示为：
[0071]
x(t+1)＝x(t)+vccosθ*t
[0072]
y(t+1)＝y(t)+vcsinθ*t
[0073]
θ(t+1)＝θ(t)+ωc*t
[0074]
其中，(x
t
，y
t
)为抓钢机t时刻的位置坐标，θ
t
抓钢机t时刻相对于目标位置的航向角。
[0075]
在本技术一实施例中，根据所述下一时间节点的位姿状态与对应轨迹参考点的误差构建抓钢机控制模型，以根据所述抓钢机控制模型控制所述左履带进行转向，包括：
[0076]
通过所述抓钢机控制模型向所述左履带下发第一实时控制信号，所述第一实时控制信号表示为：
[0077][0078]
其中，α
(
·
)
，β
(
·
)
，γ
(
·
)
，δ
(
·
)
分别表示权重系数，k
p(
·
)
，k
i(
·
)
，k
d(
·
)
分别表示可调控制参数，下标中的l，r分别表示左履带和右履带，下标v，x，y，θ分别表示履带速度和质心位姿。
[0079]
在本技术一实施例中，根据所述下一时间节点的位姿状态与对应轨迹参考点的误差构建抓钢机控制模型，以根据所述抓钢机控制模型控制所述右履带进行转向，包括：
[0080]
通过所述抓钢机控制模型向所述右履带下发第二实时控制信号，所述第二实时控制信号表示为：
[0081][0082]
其中下标中的l，r分别表示左履带和右履带，下标v，x，y，θ分别表示履带速度和质心位姿，k
p(
·
)
，k
i(
·
)
，k
d(
·
)
分别表示可调控制参数，α
(
·
)
，β
(
·
)
，γ
(
·
)
，δ
(
·
)
分别表示权重系数。
[0083]
具体地，因为经过转换后gps数据是离散点构成，这里采用分段执行的形式控制抓钢机走完全程。在每一个阶段，抽取规划轨迹中n＝t+t＝it的参考轨迹点q(n)，根据轨迹曲率、左右履带的轮间距和质心规划线速度、角速度可计算获得左右履带规划线速度；随后根据位姿计算与目标点航向角的偏差，通过航向角控制算法算出t时刻机器人角速度w(t)；同样计算t时刻与目标点位置偏差，通过距离控制算法输出t时刻机器人线速度v(t)。
[0084]
s110，最后判断是否抵近该阶段目标点，如果抵近则将该时刻位姿作为下一阶段起始位姿，进入下一阶段的轨迹跟踪；否则，更新当前位姿，不断重复进行上述操作。
[0085]
s111，根据s109所述，可得t时刻左履带的实时控制信号为
[0086][0087]
右履带的实时控制信号为：
[0088][0089]
其中下标中的l，r分别表示左履带和右履带，下标v，x，y，θ分别表示履带速度和质心位姿，k
p(
·
)
，k
i(
·
)
，k
d(
·
)
分别表示可调控制参数，α
(
·
)
，β
(
·
)
，γ
(
·
)
，δ
(
·
)
分别表示权重系数。
[0090]
请参阅图3，本实施例提供了一种无人抓钢机运动转向控制系统，用于执行前述方法实施例中所述的无人抓钢机运动转向控制方法。由于系统实施例的技术原理与前述方法实施例的技术原理相似，因而不再对同样的技术细节做重复性赘述。
[0091]
在一实施例中，无人抓钢机运动转向控制系统，包括：
[0092]
定位模块10，用于获取抓钢机实时定位信息，根据所述实时定位信息确定当前时间节点所述抓钢机的位置坐标，其中所述抓钢机为履带式抓钢机，包括左履带和右履带；
[0093]
位姿预测模块11，用于获取当前时间节点所述抓钢机的左履带和右履带的瞬时线速度，根据所述瞬时线速度、所述位置坐标建立抓钢机运动学模型，以根据所述抓钢机运动学模型确定所述抓钢机在下一时间节点的位姿状态；
[0094]
运动规划模块12，用于根据所述抓钢机初始位置和目标位置进行运动轨迹规划，得到一组不同时间节点的运动轨迹点序列，作为轨迹参考点序列；
[0095]
转向控制模块13，用于根据所述下一时间节点的位姿状态与所述轨迹参考点序列中对应轨迹参考点的误差构建抓钢机控制模型，以根据所述抓钢机控制模型控制所述左履带和右履带进行转向以使所述抓钢机向所述目标位置移动。
[0096]
上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。