CN110455290B - 一种智能液压挖掘机的最优轨迹规划方法 - Google Patents

Abstract

本发明涉及挖掘机控制技术领域，特别是涉及一种智能液压挖掘机的最优轨迹规划方法，包括采用具有超线性收敛性能的序列二次规划方法求解时间最优轨迹优化步骤，以及考虑关节动力学约束，关节加加速度半无穷约束问题和累计效果，求解最光滑轨迹优化的步骤。相较于现有技术，本发明的最优轨迹规划方法主要针对挖掘机往返路径所需时间和往返路径的光滑度进行优化，同时实现了最优时间轨迹规划和最光滑轨迹规划，不仅提高了挖掘机自动控制系统的作业效率，而且使作业轨迹更加平滑，明显降低了设备振动，延长了设备使用寿命。

Description

一种智能液压挖掘机的最优轨迹规划方法

技术领域

本发明涉及挖掘机控制技术领域，特别是涉及一种智能液压挖掘机的最优轨迹规划方法。

背景技术

挖掘机被广泛应用于在矿山开采、建筑破拆、路桥建设的施工工地。现有技术中，挖掘机普遍采用液压系统驱动，人工操控的方式进行施工。但是由于操作挖掘机这种大型机器较为复杂，施工工地环境通常又较为恶劣，不同操作人的技术和经验存在较大差别等，导致人工操作挖掘机效率低且工作难以达到统一要求。因此，能够进行无人作业的智能挖掘机逐渐得到发展。为实现挖掘机的智能化，提高挖掘机自主挖掘能力，需确定合理的挖掘作业轨迹规划。

轨迹规划是智能挖掘机控制系统的一项基本任务，通常情况下，挖掘机作业轨迹是由运动段和过渡段组成的。运动段是一条直线、圆弧或者椭圆形状，过渡段则是在不同形状的运动段之间切换时的轨迹。挖掘机在施工时，除了要求设备在部分轨迹具有严格的位置和速度以外，还要求在过渡轨迹段，具有较高的作业效率，以及较为平滑的运动轨迹。

挖掘机作业的主要目的之一就是提高生产作业效率，因此时间优化是轨迹优化中极为重要的一环。在工业领域，通常将该时间优化指标定义为如何减少获得任务的设备从初始位置运动到工作位置的时间。当挖掘机运动轨迹不连续时，会增大关节运动跟踪误差、引起振动和缩短设备使用寿命，因此，在进行轨迹规划时除了要考虑时间优化，还要考虑光滑轨迹优化。

现有技术中，虽然公开了不少挖掘机轨迹规划方法，有的轨迹规划方法达到了最优时间控制，但是运动轨迹不够平滑，对设备冲击较大，振动明显，缩短了挖掘机工作装置的使用寿命；而另一些轨迹规划方法虽然减振效果明显，但是计算复杂，轨迹误差大，效率低。

发明内容

有鉴于此，本发明的目的是克服现有技术的缺陷，提供一种智能挖掘机的最优轨迹规划方法，实现了挖掘机光滑运动轨迹的时间最优控制。

为了达到上述目的，本发明采用以下的技术方案：

所述最优轨迹规划方法

包括以下步骤：

(1)根据挖掘机定位及作业目标位置，进行路径规划并在参数空间中离散化；

(2)进行逆运动学求解；

(3)进行逆动力学求解；

(4)构造并求解轨迹优化；

(5)获得最优轨迹；

其中主要对挖掘机往返路径所需时间和往返路径的光滑度进行优化：

A、时间最优轨迹优化：

挖掘机工作装置各个关节的运动速度、加速度和加加速度曲线方程通过对路径规划系统基于B样条曲线所自动生成的运动轨迹求导获得：

各个运动关节的7次B样条轨迹曲线的控制顶点满足下列条件：

式中

分别为第m个关节的B样条速度、加速度和加加速度曲线的第j个控制顶点，由德布尔递推公式求出：

将关节速度、加速度和加加速度的约束转化为B样条轨迹曲线控制顶点的约束，有效避免了对轨迹曲线采样的半无穷约束问题；

挖掘机轨迹优化系统的时间最优轨迹优化就是在满足设备本体动力学约束条件下，求解系统运动总时间最小的时间节点序列：

式中：

Δt＝[Δt₀，Δt₁，…，Δt_n-1]^T，i＝0，1，…，n-1

令：

根据k₁、k₂和k₃确定时间节点向量的初始值：

在轨迹优化系统中采用具有超线性收敛性能的序列二次规划方法求解时间最优轨迹优化，具体流程为：

A-1、构造Lagrange函数将非线性约束线性化，确定初始值；

A-2、计算Hessian矩阵；

A-3、模拟Newton-Raphson，得到k个二次规划子问题；

A-4、求解二次规划子问题；若满足条件，则更新目标值、Hessian矩阵，返回步骤A-3；若不满足条件，则重新求解二次规划子问题；

B、最光滑轨迹优化：

考虑关节动力学约束，以及关节加加速度半无穷约束问题和累计效果，求解最光滑轨迹优化：

其中k_m为根据关节保护确定的权重系数，J(Δt)为加加速度累计指数，Δt为时间节点向量，Δt^*和J(Δt^*)分别为最优光滑轨迹对应的时间节点向量和全局最小加加速度累计指数；T_def为根据作业时序要求指定的轨迹执行时间，kc为考虑7次B样条曲线的强凸包性质而采用的运动学约束放大倍数；T_def必须大于或等于最小执行时间，最小执行时间可通过时间最优轨迹优化问题得到；

通过以下计算公式计算各关节液压装置的油缸推力和拉力、伸出和缩回速度，然后根据获得的最优轨迹，挖掘机的中央控制器发出控制信号控制工作装置进行作业：

油缸推力：

油缸拉力：

伸出速度：

缩回速度：

式中：F1——在无杆腔产生的力(推力)，kgf；

F2——在有杆腔产生的力(拉力)，kgf；

A、B——无杆腔、有杆腔面积，cm²；

D——油缸内径，cm；

d——活塞杆直径，cm；

V1——活塞杆伸出速度，cm/min；

V2—一活塞杆缩回速度，cm/min；

Q1——油缸无杆腔侧进油流量，cm³/min；

Q2——油缸有杆腔侧进油流量，cm³/min。

优选地，在时间最优轨迹规划优化中，采用具有超线性收敛性能的序列二次规划方法求解时间最优轨迹优化，具体求解过程为：

L(x，λ)＝f(x)-λ^TC(x)

上式为线性化的轨迹序列，其中，λ＝[λ₁，λ₂，...，λ_3N]^T为Lagrange乘子，C(x)＝[c₁(x)，c₂(x)，...，c_3N(x)]^T；当Lagrange函数的梯度为：

为非线性优化问题的K-T点，也就是时间最优问题的解；通过Newton-Raphson法，得到序列二次规划方法的第k个二次规划子问题：

式中：

B_k∈R^n×n式Lagrange函数的Hessian矩阵的近似，及第k次二次规划问题的解为d_k，有如下K-T方程成立：

求上述k次二次规划问题的解d_k，若这一二次规划问题解d_k的范数||d_k||小于预设精度值，则算法停止；否则求解平均最小精确惩罚函数解：

为了克服精确罚函数作为价值函数时可能遇到破坏序列二次规划超线性收敛性能的Maratos效应，在计算过程中会引入一个二阶校正步骤；

最后计算下一最优时间，并利用BFGS方法更新Hessian矩阵：

本发明的技术方案达到以下有益效果：

相较于现有技术，本发明智能液压挖掘机的最优轨迹优化方法主要针对挖掘机往返路径所需时间和往返路径的光滑度进行优化，同时实现了最优时间轨迹规划和最光滑轨迹规划，不仅提高了挖掘机自动控制系统的作业效率，而且使作业轨迹更加平滑，明显降低了设备振动，延长了设备使用寿命。

附图说明

图1为实施例的最优轨迹规划方法流程图；

图2为实施例的时间最优轨迹规划优化的具体流程图；

图3为实施例的液压挖掘机的液压缸的工作原理示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

参照图1至图3，以智能液压挖掘机进行自主挖掘作业为例，该智能液压挖掘机包括中央控制器、视觉模块、工作模块、动力模块、电比例控制多路阀、与所述电比例控制多路阀连接的挖掘机主阀，所述电比例控制多路阀与所述中央控制器连接；其中：视觉模块用于为所述中央控制器提供图像信号，其包括与所述中央控制器连接的图像处理器以及设置于挖掘机本体上的摄像头，所述摄像头与所述图像处理器连接；工作模块为执行机构，其包括回转装置、动臂、斗杆和工作头，所述中央控制器通过角位移传感器获得所述回转装置的转动角度，并通过各个倾角传感器分别获得所述动臂、斗杆和工作头的倾角；动力模块包括发动机、液压变量泵和挖掘机主阀，所述挖掘机主阀为所述工作模块提供动力。该智能液压挖掘机还包括与所述中央控制器连接的电控挡位器、与所述电控挡位器连接的挖掘机控制器，所述挖掘机控制器分别与所述发动机和所述液压变量泵连接，所述液压变量泵与所述挖掘机主阀连接。同时，在本实施例中，挖掘机的本体上还设置有GPS定位模块，将GPS定位模块与中央控制器连接，使得中央控制器在控制挖掘机行走时能精确定位。安装在挖掘机本体上的GPS定位模块能够获取挖掘机本体的实时三维坐标(x，y，z)以及挖掘机本体上车部的方向角。

本实施例还引入了轨迹优化系统，该轨迹优化系统至少包括与中央处理器连接的轨迹规划器，用于接收各传感器的信号，以确定工作装置的各关节的位姿、速度、加速度，以及各关节的液压装置的油压、油液流量、液压杆的伸缩速度等参数，并进行最优轨迹规划。得到的最优轨迹规划发送给中央处理器，中央处理器根据最优轨迹规划发出控制信号，控制工作装置进行作业。

如图1所示，本实施例的最优轨迹规划方法具体为：

(1)根据挖掘机定位及作业目标位置，进行路径规划并在参数空间中离散化；

(2)进行逆运动学求解；

(3)进行逆动力学求解；

(4)构造并求解轨迹优化；

(5)获得最优轨迹；

其中主要对挖掘机往返路径所需时间和往返路径的光滑度进行优化：

所述步骤(4)具体包括：

A、时间最优轨迹优化：

挖掘机工作装置各个关节的运动速度、加速度和加加速度曲线方程通过对路径规划系统基于B样条曲线所自动生成的运动轨迹求导获得：

各个运动关节的7次B样条轨迹曲线的控制顶点满足下列条件：

式中

分别为第m个关节的B样条速度、加速度和加加速度曲线的第j个控制顶点，由德布尔递推公式求出：

将关节速度、加速度和加加速度的约束转化为B样条轨迹曲线控制顶点的约束，有效避免了对轨迹曲线采样的半无穷约束问题；

挖掘机轨迹优化系统的时间最优轨迹优化就是在满足设备本体动力学约束条件下，求解系统运动总时间最小的时间节点序列：

式中：

Δt＝[Δt₀，Δt₁，…，Δt_n-1]^T，i＝0，1，…，n-1

令：

根据k₁、k₂和k₃确定时间节点向量的初始值：

在轨迹优化系统中采用具有超线性收敛性能的序列二次规划方法求解时间最优轨迹优化，如图2所示，具体流程为：

A-1、构造Lagrange函数将非线性约束线性化，确定初始值；

A-2、计算Hessian矩阵；

A-3、模拟Newton-Raphson，得到k个二次规划子问题；

A-4、求解二次规划子问题；若满足条件，则更新目标值、Hessian矩阵，返回步骤A-3；若不满足条件，则重新求解二次规划子问题。

L(x，λ)＝f(x)-λ^TC(x)

上式为线性化的轨迹序列，其中，λ＝[λ₁，λ₂，...，λ_3N]^T为Lagrange乘子，C(x)＝[c₁(x)，c₂(x)，...，c_3N(x)]^T；当Lagrange函数的梯度为：

为非线性优化问题的K-T点，也就是时间最优问题的解；通过Newton-Rap hson法，得到序列二次规划方法的第k个二次规划子问题：

式中：

B_k∈R^n×n式Lagrange函数的Hessian矩阵的近似，及第k次二次规划问题的解为d_k，有如下K-T方程成立：

求上述k次二次规划问题的解d_k，若这一二次规划问题解d_k的范数||d_k||小于预设精度值，则算法停止；否则求解平均最小精确惩罚函数解：

为了克服精确罚函数作为价值函数时可能遇到破坏序列二次规划超线性收敛性能的Maratos效应，在计算过程中会引入一个二阶校正步骤；

最后计算下一最优时间，并利用BFGS方法更新Hessian矩阵：

B、最光滑轨迹优化：

考虑关节动力学约束，以及关节加加速度半无穷约束问题和累计效果，求解最光滑轨迹优化：

其中k_m为根据关节保护确定的权重系数，J(Δt)为加加速度累计指数，Δt为时间节点向量，Δt^*和J(Δt^*)分别为最优光滑轨迹对应的时间节点向量和全局最小加加速度累计指数；T_def为根据作业时序要求指定的轨迹执行时间，kc为考虑7次B样条曲线的强凸包性质而采用的运动学约束放大倍数；T_def必须大于或等于最小执行时间，最小执行时间可通过时间最优轨迹优化问题得到；

如图3所示，通过以下计算公式计算各关节液压装置的油缸推力和拉力、伸出和缩回速度，然后根据获得的最优轨迹，挖掘机的中央控制器发出控制信号控制工作装置进行作业：

油缸推力：

油缸拉力：

伸出速度：

缩回速度：

式中：F1——在无杆腔产生的力(推力)，kgf；

F2——在有杆腔产生的力(拉力)，kgf；

A、B——无杆腔、有杆腔面积，cm²；

D——油缸内径，cm；

d一—活塞杆直径，cm；

V1—一活塞杆伸出速度，cm/min；

V2——活塞杆缩回速度，cm/min；

Q1——油缸无杆腔侧进油流量，cm³/min；

Q2——油缸有杆腔侧进油流量，cm³/min。

上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。

Claims (2)

1.一种智能液压挖掘机的最优轨迹规划方法，其特征在于，包括以下步骤：

(1)根据挖掘机定位及作业目标位置，进行路径规划并在参数空间中离散化；

(2)进行逆运动学求解；

(3)进行逆动力学求解；

(4)构造并求解轨迹优化；

(5)获得最优轨迹；

其中主要对挖掘机往返路径所需时间和往返的光滑度进行优化：

所述步骤(4)具体包括：

A、时间最优轨迹优化：

挖掘机工作装置各个关节的运动速度、加速度和加加速度曲线方程通过对路径规划系统基于B样条曲线所自动生成的运动轨迹求导获得：

各个运动关节的7次B样条轨迹曲线的控制顶点满足下列条件：

式中

分别为第m个关节的B样条速度、加速度和加加速度曲线的第j个控制顶点，由德布尔递推公式求出：

将关节速度、加速度和加加速度的约束转化为B样条轨迹曲线控制顶点的约束，有效避免了对轨迹曲线采样的半无穷约束问题；

挖掘机轨迹优化系统的时间最优轨迹优化就是在满足设备本体动力学约束条件下，求解系统运动总时间最小的时间节点序列：

式中：

Δt＝[Δt₀，Δt₁，…，Δt_n-1]^T，i＝0，1，…，n-1

令：

根据k₁、k₂和k₃确定时间节点向量的初始值：

在轨迹优化系统中采用具有超线性收敛性能的序列二次规划方法求解时间最优轨迹优化，具体流程为：

A-1、构造Lagrange函数将非线性约束线性化，确定初始值；

A-2、计算Hessian矩阵；

A-3、模拟Newton-Raphson，得到k个二次规划子问题；

A-4、求解二次规划子问题；若满足条件，则更新目标值、Hessian矩阵，返回步骤A-3；若不满足条件，则重新求解二次规划子问题；

B、最光滑轨迹优化：

考虑关节动力学约束，以及关节加加速度半无穷约束问题和累计效果，求解最光滑轨迹优化：

其中k_m为根据关节保护确定的权重系数，J(Δt)为加加速度累计指数，Δt为时间节点向量，Δt^*和J(Δt^*)分别为最优光滑轨迹对应的时间节点向量和全局最小加加速度累计指数；T_def为根据作业时序要求指定的轨迹执行时间，kc为考虑7次B样条曲线的强凸包性质而采用的运动学约束放大倍数；T_def必须大于或等于最小执行时间，最小执行时间通过时间最优轨迹优化问题得到；

通过以下计算公式计算各关节液压装置的油缸推力和拉力、伸出和缩回速度，然后根据获得的最优轨迹，挖掘机的中央控制器发出控制信号控制工作装置进行作业：

油缸推力：

油缸拉力：

伸出速度：

缩回速度：

式中：F1——在无杆腔产生的推力，kgf；

F2——在有杆腔产生的拉力，kgf；

A、B——无杆腔、有杆腔面积，cm²；

D——油缸内径，cm；

d——活塞杆直径，cm；

V1——活塞杆伸出速度，cm/min；

V2——活塞杆缩回速度，cm/min；

Q1——油缸无杆腔侧进油流量，cm³/min；

Q2——油缸有杆腔侧进油流量，cm³/min。

2.根据权利要求1所述的智能液压挖掘机的最优轨迹规划方法，其特征在于：采用具有超线性收敛性能的序列二次规划方法求解时间最优轨迹优化，具体求解过程为：

L(x，λ)＝f(x)-λ^TC(x)

上式为线性化的轨迹序列，其中，λ＝[λ₁，λ₂，...，λ_3N]^T为Lagrange乘子，C(x)＝[c₁(x)，c₂(x)，...，c_3N(x)]^T；当Lagrange函数的梯度为：

为非线性优化问题的K-T点，也就是时间最优问题的解；通过Newton-Raphson法，得到序列二次规划方法的第k个二次规划子问题：

式中：

B_k∈R^n×n式Lagrange函数的Hessian矩阵的近似，及第k次二次规划问题的解为d_k，有如下K-T方程成立：

求上述k次二次规划问题的解d_k，若这一二次规划问题解d_k的范数||d_k||小于预设精度值，则算法停止；否则求解平均最小精确惩罚函数解：

为了克服精确罚函数作为价值函数时可能遇到破坏序列二次规划超线性收敛性能的Maratos效应，在计算过程中引入一个二阶校正步骤；

最后计算下一最优时间，并利用BFGS方法更新Hessian矩阵：

Images