CN113589809A - 可避障的挖掘机工作装置作业轨迹规划方法及装置 - Google Patents

Abstract

本发明公开了一种可避障的挖掘机工作装置作业轨迹规划方法及装置，所述方法包括：生成挖掘机工作装置的运动避障路径点；基于所述运动避障路径点，进行挖掘机工作装置运动轨迹规划，生成最优路径；基于所述最优路径，控制挖掘机工作装置的运动。本发明不仅使挖掘机工作装置实现运动过程中的避障避碰，而且满足挖掘机工作装置各个节点的运行学约束，同时能达到时间最优的目标，从而提高挖掘机工作装置的工作效率，缓解挖掘操作对专业性人才依赖的限制。

Description

可避障的挖掘机工作装置作业轨迹规划方法及装置

技术领域

本发明属于挖掘臂作业轨迹规划技术领域，具体涉及一种可避障的挖掘机工作装置作业轨迹规划方法及装置。

背景技术

随着社会的不断进步发展，为了更好的为人类服务，挖掘机的作业质量要求也亟待提高，如何完善其工作效率及工作可靠性成为当务之急。而对挖机进行轨迹规划，使其能够快速平稳的完成挖掘任务，则能有效解决上述问题，从而提高了挖掘机的整体性能。轨迹规划研究的主要内容是首先根据环境信息，通过避障算法得到避障路径点，然后在此基础上规划挖机臂每个关节的位移、速度以及加速度问题，最终使挖机臂末端完成一定工况下的作业。挖机臂轨迹规划是挖机臂正运动学问题和逆运动学问题的实际应用，因此，对挖机臂进行轨迹规划时，必须遵循一定的原则并采用合理的方法，避免其位移、速度以及加速度的突变。

因此进行挖机臂的轨迹优化研究是对其进行智能化改造升级的重要组成部分，它可促进挖掘机的产业结构调整，而且挖掘机智能化的进步也必将带动其他行业的发展，这对工业领域的自动化技术实现乃至国民经济的全面发展都有极其重要的意义。

公开号为CN107186713A的中国发明专利申请中公开了，通过一种路径点的机械臂多轴运动规划优化方法，求解最优的运动时间序列，将该时间序列带入机械臂关节运动轨迹方程，进而得到路径点的机械臂多轴连续运动的最佳规划曲线。该方案中没有考虑机械臂运动过程中的避障问题，在实际应用过程中会有较大的风险。

公开号为CN108356819A的中国发明专利申请中公开了，将传统的A*算法的搜索步长设置为可变步长，采用基于分离轴的碰撞检测算法在搜索的每一步中判断机械臂是否与环境发生碰撞，在六自由度机械臂多维空间中进行搜索，得到无碰撞运动路径。该方案中未进行最优化处理过程，例如时间最优、能耗最优等优化处理。

发明内容

针对上述问题，本发明提出一种可避障的挖掘机工作装置作业轨迹规划方法及装置，不仅使挖掘机工作装置实现运动过程中的避障避碰，而且满足挖掘机工作装置各个节点的运行学约束，同时能达到时间最优的目标，从而提高挖掘机工作装置的工作效率，缓解挖掘操作对专业性人才依赖的限制。

为了实现上述技术目的，达到上述技术效果，本发明通过以下技术方案实现：

第一方面，本发明提供了一种可避障的挖掘机工作装置作业轨迹规划方法，包括：

生成挖掘机工作装置的运动避障路径点；

基于所述运动避障路径点，进行挖掘机工作装置运动轨迹规划，生成最优路径；

基于所述最优路径，控制挖掘机工作装置的运动。

可选地，所述运动避障路径点的生成方法包括：

获得障碍物的位置信息以及形状信息；

基于挖掘机工作装置的运动起始点和运动目标点的坐标信息，以及所述障碍物的位置信息以及形状信息，规划出避障路径点。

可选地，所述基于挖掘机工作装置的运动起始点和运动目标点的坐标信息，以及所述障碍物的位置信息以及形状信息，规划出避障路径点，具体为：

根据障碍物的位置信息以及形状信息，设置障碍物的栅格位置以及面积；

根据挖机工作装置末端的运动起始点和运动目标点的坐标信息，将挖掘工作装置运动空间栅格化后，取栅格中心为节点；

利用A*算法，基于障碍物的栅格位置、面积，以及栅格化后的挖掘工作装置运动空间，根据节点的优先级，进行避障的路径规划。

可选地，所述A*算法通过以下函数来计算每个节点的优先级：

F(n)＝G(n)+H(n)

其中，F(n)是节点n的综合优先级，G(n)是节点n距离起点的代价，H(n)是节点n距离终点的预计代价，当选择下一个要遍历的节点时，总会选取综合优先级值最小的节点。

可选地，所述最优路径的生成方法包括：

基于所述运动避障路径点，利用插值函数分段建立三次多项式；

以动作时间最短为目标函数，以运动速度、加速度、脉动曲线不超过相应约束范围为约束条件，求解出三次多项式的各项系数，得到最优路径。

可选地，所述三次多项式为：

P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0

其中，P_ji(t)表示的是第j个关节的第i段轨迹表达式，在N个关节的机械臂中j＝1,2,3,…,N，当挖掘机工作装置从起始点至目标点共计有n个中间点时，每个关节的轨迹可以划分为第i＝1,2,3,…,n-1段；s_ji3表示的是第j个关节、第i段轨迹中三次项的系数，s_ji2表示的是第j个关节、第i段轨迹中二次项的系数，s_ji1表示的是第j个关节、第i段轨迹中一次项的系数，s_ji0表示的是j第个关节、第i段轨迹中常数项的系数；

所述目标函数为：

所述约束条件为：

其中，g为工作装置运动所用的总时间，P_j'_i(t)，P_j”_i(t)，P_j”_i'(t)分别表示轨迹函数P_ji(t)的一，二，三阶导数，V_j,max为第j个关节的速度约束条件，W_j,max为第j个关节的加速度约束条件，K_j,max为第j个关节的脉动约束条件，h_j,i＝t_j,i+1-t_j,i为第j个关节在第i段轨迹所需要的时间。

可选地，所述利用插值函数分段建立三次多项式步骤之前还包括：

以所述运动避障路径点为约束条件，以挖掘机工作装置各关节的角度变化量之和最小为目标，筛选出最优的路径点；

则所述三次多项式基于筛选出最优的路径点利用插值函数分段建立。

可选地，所述最优的路径点的筛选方法具体为：

对挖掘机工作装置所处坐标系进行旋转和平移变换，获得第i个坐标系相对于车身坐标系的齐次变换矩阵T_i ⁰；

利用逆运动学方法，根据挖掘机工作装置末端的位置，求取挖机工作装置中各关节变量，其中，在逆运动学方法中，目标函数min(|α_i-α_i-1|+|β_i-β_i-1|+|θ_i-θ_i-1|)，其中α_i，β_i，θ_i分别表示三个关节在第i段轨迹运动的角度，所述挖掘机工作装置末端的位置是基于将GPS坐标系下的数据转换成当地水平坐标系中的数据。

可选地，所述齐次变换矩阵T_i ⁰的计算公式为：

第二方面，本发明提供了一种可避障的挖掘机工作装置作业轨迹规划装置，包括：

避障单元，生成挖掘机工作装置的运动避障路径点；

规划单元，基于所述运动避障路径点，进行挖掘机工作装置运动轨迹规划，生成最优路径；

控制单元，基于所述最优路径，控制挖掘机工作装置的运动。

与现有技术相比，本发明的有益效果：

本发明在求解最优化路径过程中，将工作装置运动速度、加速度、脉动速度作为约束条件，并将运动时间最短作为优化目标，在实现挖掘机自主避障挖掘的同时，大大提高了挖掘机的工作效率，缓解了挖掘机作业对专业性人才依赖的限制。

附图说明

为了使本发明的内容更容易被清楚地理解，下面根据具体实施例并结合附图，对本发明作进一步详细的说明，其中：

图1为本发明一种实施例的实现流程图；

图2为本发明一种实施例的各传感器的安装示意图；

图3为本发明一种实施例的挖机臂坐标系；

图4为本发明一种实施例的A*算法流程图；

图5为本发明一种实施例的粒子群算法流程图；

图6为本发明一种实施例的遗传算法流程图；

其中：1-激光雷达，2-长距毫米波雷达，3-工控机，4-倾角传感器，5-长度传感器，6-惯性导航***及控制器，7-GPS天线。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明的保护范围。

下面结合附图对本发明的应用原理作详细的描述。

实施例1

本发明实施例中提供了一种可避障的挖掘机工作装置作业轨迹规划方法，具体包括以下步骤：

步骤(1)生成挖掘机工作装置的运动避障路径点；

步骤(2)基于所述运动避障路径点，进行挖掘机工作装置运动轨迹规划，生成最优路径；

步骤(3)基于所述最优路径，控制挖掘机工作装置的运动。

在本发明实施例的一种具体实施方式中，所述运动避障路径点的生成方法包括：

获得障碍物的位置信息以及形状信息；

基于挖掘机工作装置的运动起始点和运动目标点的坐标信息，以及所述障碍物的位置信息以及形状信息，规划出避障路径点；

在具体实施过程中，为了采集障碍物的位置信息以及形状信息，需要进行传感器的布置，可以采用如图2所示的布置方式：

1)挖掘机正前方安装：2台激光雷达1、1台长距毫米波雷达2；

2)挖掘机车体两侧对称安装：各1台激光雷达1；

3)挖掘机后方安装：2台激光雷达1、1台长距毫米波雷达2；

4)驾驶室顶部安装：1台激光雷达1；

5)挖掘机车内部安装：1台交换机、1台工控机3；

6)挖掘机臂安装：1台激光雷达1；

7)挖掘机工作装置安装：倾角传感器4、长度传感器5；所述挖掘机工作装置包括：动臂、斗杆和铲斗；

8)挖掘机上车安装惯性导航***(采用的是GPS坐标系)及控制器6、GPS天线7。

所述基于挖掘机工作装置的运动起始点和运动目标点的坐标信息，以及所述障碍物的位置信息以及形状信息，规划出避障路径点，具体包括以下步骤：

根据障碍物的位置信息以及形状信息，设置障碍物的栅格位置以及面积；

根据挖机工作装置末端的运动起始点和运动目标点的坐标信息，将挖掘工作装置运动空间栅格化后，取栅格中心为节点；

利用A*算法，基于障碍物的栅格位置、面积，以及栅格化后的挖掘工作装置运动空间，根据节点的优先级，进行避障的路径规划，所述A*算法通过以下函数来计算每个节点的优先级：

F(n)＝G(n)+H(n)

其中，F(n)是节点n的综合优先级，G(n)是节点n距离起点的代价，H(n)是节点n距离终点的预计代价，当选择下一个要遍历的节点时，总会选取综合优先级值最小的节点。

A*算法的流程图见图4，具体步骤如下：

步骤1：初始化open列表和close列表为空，将起点加入到open列表中；open列表如果为空，那么结束，否则继续；

步骤2：计算open列表中每个节点的F值，F＝G+H，H的计算使用Manhattan方法，找出open列表中F值最小的节点n，同时根据F值更新每个节点的父节点；

步骤3：将节点n从open列表中移除，添加到close列表中；

步骤4：节点n如果是目标节点，那么得到路径，从终点回溯到起点，否则继续；

步骤5：节点n如果没有子节点，那么返回步骤2，否则继续；

步骤6：查找节点n相邻的节点，跳过close列表中的节点以及不可到达的节点，将这些相邻的节点加入到open列表中，返回步骤2。

步骤7：open列表中的节点组成了可行的路径点,该路径点就对应每个栅格的中心。

在本发明实施例的一种具体实施方式中，所述最优路径的生成方法包括：

以所述运动避障路径点为约束条件，以挖掘机工作装置各关节的角度变化量之和最小为目标，筛选出最优的路径点；

基于所述最优的路径点，利用插值函数分段建立三次多项式；

以动作时间最短为目标函数，以运动速度、加速度、脉动曲线不超过相应约束范围为约束条件，求解出三次多项式的各项系数，得到最优路径。

所述三次多项式为：

P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0

其中，P_ji(t)表示的是第j个关节的第i段轨迹表达式，即轨迹函数；在N个关节的机械臂中j＝1,2,3,…,N，当挖掘机工作装置从起始点至目标点共计有n个中间点时，每个关节的轨迹可以划分为第i＝1,2,3,…,n-1段；s_ji3表示的是第j个关节、第i段轨迹中三次项的系数，s_ji2表示的是第j个关节、第i段轨迹中二次项的系数，s_ji1表示的是第j个关节、第i段轨迹中一次项的系数，s_ji0表示的是j第个关节、第i段轨迹中常数项的系数；

所述目标函数为：

所述约束条件为：

其中，g为工作装置运动所用的总时间，P_j'_i(t)，P_j”_i(t)，P_j”_i'(t)分别表示轨迹函数P_ji(t)的一，二，三阶导数，V_j,max为第j个关节的速度约束条件，W_j,max为第j个关节的加速度约束条件，K_j,max为第j个关节的脉动约束条件，h_j,i＝t_j,i+1-t_j,i为第j个关节在第i段轨迹所需要的时间。

在本发明实施例的一种具体实施方式中，所述最优的路径点的筛选方法具体为：

对挖掘机工作装置所处坐标系进行旋转和平移变换，获得第i个坐标系相对于车身坐标系的齐次变换矩阵T_i ⁰；

利用逆运动学方法，根据挖掘机工作装置末端的位置，求取挖机工作装置中各关节变量，其中，在逆运动学方法中，目标函数min(|α_i-α_i-1|+|β_i-β_i-1|+|θ_i-θ_i-1|)，其中α_i，β_i，θ_i分别表示三个关节在第i段轨迹运动的角度，所述挖掘机工作装置末端的位置是基于将GPS坐标系下的数据转换成当地水平坐标系中的数据。

所述齐次变换矩阵T_i ⁰的计算公式为：

具体地，所述最优路径的生成过程为：

1)挖掘机工作装置位姿与运动描述：

挖掘机工作装置的坐标系见图3，其中坐标系的建立符合右手定则，坐标系建立后，可以通过4个步骤将坐标系o_i-1x_i-1y_i-1z_i-1变换为下一个坐标系o_ix_iy_iz_i：

步骤1：绕z_i-1轴旋转θ_i度，将x_i-1和x_i轴变为相同方向，此变换矩阵记作Rot(Z,θ_i)，Z表示围绕Z轴进行旋转；

步骤2：沿z_i-1轴平移d_i的单位，使x_i-1和x_i轴共线，此变换矩阵记作Trans(0,0,d_i)；

步骤3：沿x_i-1轴平移a_i的单位，使两个坐标系的原点重合，此变换矩阵记作Trans(α_i,0,0)；

步骤4：绕x_i轴旋转a_i度，使z_i-1和z_i轴共线，两个坐标系随之完全重合，此变换矩阵记作Rot(X,α_i),X表示围绕X轴进行旋转；

第i-1个坐标系和第i个坐标系的关系可用矩阵

表示，其中：

根据平移矩阵和旋转齐次变换公式，可得：

因此

第i个坐标系相对于车身坐标系的齐次变换矩阵T_i ⁰可以用各连续坐标系齐次变换矩阵的乘积表示，即

在本发明实施例中，利用逆运动学方法，根据挖机工作装置末端(即铲斗齿尖末端)的位置，求取挖机工作装置各关节变量。由于挖机工作装置只有三个关节，因此可得：

根据上式，可得

其中，E为单位矩阵。

所述挖掘机工作装置末端的位置是基于将GPS坐标系下的数据转换成当地水平坐标系中的数据，具体转换过程为：

车辆位置与其周围障碍物位置采用绝对坐标系下的坐标位置--地球坐标系。由于通过GPS天线和惯导读到的GPS数据是地理坐标系(WGS84)，需要对该GPS数据进行解析与坐标变换，转换成以X轴指向正东，Y轴指向正北，原点位于惯性导航单元质心的直角坐标系数据；解析与坐标变换既可以通过高斯正算公式的计算实现。具体方法如下：

惯性导航单元可将采集到的GPS数据输出给工控机，GPS数据采用GPRMC格式(即最小定位信息)，包含了时间日期、经纬度、航向、磁偏角等信息；在***中创建GPS数据结构体，对得到的GPRMC数据进行数据解析，得到经度(L)和纬度(B)数据；由于采集到GPS数据是地球坐标，不能直接给挖掘机使用，需要通过高斯投影的方法，将L、B坐标通过高斯投影(高斯正算公式)得到高斯平面直角坐标，高斯正算公式可实现的精度为0.1米，具体公式如下所示：

X＝m₀+m₂l²+m₄l⁴

Y＝n₁l+n₃l³+n₅l⁵

式中l是经差，m₀,n₁,m₂,...是待定系数，他们是维度B的函数；(X,Y)为转换后的坐标。然后再将高斯平面直角坐标，转换成当地的水平坐标，则得到的坐标系y轴向东为正，x轴向北为正，与习惯的坐标系定义不统一，通过坐标转换可得到通用坐标系。转换公式如下：

通过上述方法将WGS84坐标系下的GPS数据转换到X轴指向正东，Y轴指向正北，原点位于传感器质心的当地水平坐标系中。

跟据转换后的车身GPS数据，且挖掘机工作装置各关节角度可通过传感器数据采集，通过机械臂正运动学，便可解算得到挖掘机工作装置末端位置及姿态。

在基于获得的路径点进行挖机工作装置运动逆解计算过程中，本发明的优化函数为各关节的角度变化量之和最小，即运动路最短，即min(|α_i-α_i-1|+|β_i-β_i-1|+|θ_i-θ_i-1|)，约束条件为前述的路径规划，其中α_i，β_i，θ_i分别表示三个关节在第i段轨迹运动的角度，然后采用粒子群优化算法进行优化求解，其中粒子群优化算法的流程图见图5。算法具体步骤如下：

步骤1：初始化一群微粒，包括随机位置与速度；

步骤2：评价每个微粒的适应度；

步骤3：对每个微粒，将其适应值与其经过的最好位置pbest(即：局部最优位置)作比较，如果较好，则将其作为当前的最好位置gbest(即：全局最优位置)；

步骤4：根据步骤2、步骤3式调整微粒速度与位置；

步骤5：未达到迭代终止条件则转步骤2继续。

迭代终止条件选为最大迭代次数或微粒群在最大迭代次数前搜索到的最优位置，即结果收敛，筛选出最优的路径点。

2)计算挖掘机工作装置的轨迹函数

三次多项式插值函数(即三次样条插值函数)是可以保证位置、速度、加速度连续的阶次最低多项式，因而成为使用广泛、计算简单的方法之一。因此釆用三次多项式样条函数方法在关节空间中对挖掘机运动轨迹进行规划，可以使受控变量与运动时间相关，轨迹规划时计算过程简单方便。

三次样条多项式轨迹的一般形式可以表示为：

P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0

其中，P_ji(t)表示的是第j个关节的第i段轨迹表达式，在N个关节的机械臂中j＝1,2,3,…,N，当机械臂从起始点至目标点共计有n个中间点(包含起始点与终止点)时，每个关节的轨迹可以划分为第i＝1,2,3,…,n-1段。s_ji3表示的是第j个关节、第i段轨迹中三次项的系数，s_ji2表示的是第j个关节、第i段轨迹中二次项的系数，s_ji1表示的是第j个关节、第i段轨迹中一次项的系数，s_ji0表示的是j第个关节、第i段轨迹中常数项的系数。由于末端执行器是各个关节共同作用的结果，因每个关节都有n-1段轨迹，所以末端执行器的运动路径也分为n-1段。

3)基于适应值函数的挖掘机工作装置轨迹规划

轨迹规划的最终目的就是求出P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0中各项的系数，并保证将所求出的各个系数代入P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0后所得的关节角度、速度、加速度、脉动曲线中，每一条曲线均能满足其相应的约束条件。

对挖掘机的工业应用而言，它的运动速度与生产效率密切相关，为了最大化提高挖掘机效率，本发明把它的动作时间优化至最短。即在轨迹函数表达式求解过程中，除了使各关节轨迹满足运动学约束条件之外，还要保证总的运动时间最短。此问题的实质就是如何合理地求解挖掘机工作装置工作时两个相邻节点间的时间间隔，从而使所有时间间隔的总和总运动时间最小的问题。除此之外，当用优化出的时间进行轨迹规划时，必须保证轨迹的速度、加速度、脉动曲线不能超过相应约束范围，否则，优化出的时间是无效的，必须重新进行轨迹的规划。

本发明将所有关节同时进行优化，此方法的特点是求解过程比较复杂，但是所有关节运动时间均相同，动作连续性好，控制简单。

对具有N个关节的挖机臂，进行如下定义：

优化目标函数为：

约束条件为：

其中，g为工作装置运动所用的总时间，P_j'_i(t)，P_j”_i(t)，P_j”_i'(t)分别表示轨迹函数P_ji(t)的一，二，三阶导数，V_j,max为第j个关节的速度约束条件，W_j,max为第j个关节的加速度约束条件，K_j,max为第j个关节的脉动约束条件，h_j,i＝t_j,i+1-t_j,i为第j个关节在第i段轨迹所需要的时间,

将总时间作为优化的适应值函数，进行寻优求解。本发明利用遗传算法进行求解，见图6所示。如果遗传算法搜寻到的解满足约束条件，则计算适应值后继续进行迭代，如果遗传算法搜寻的解不满足约束条件，则将适应值设置为一个极大的数。

具体步骤如下：

步骤1：创建初始化某固定数量的时间种群，种群里的个体即为各关节运动时间；

步骤2：通过时间种群更新轨迹函数式各项系数；

步骤3：将时间种群中的时间间隔带入P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0，计算P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0相关约束条件的最大值；

步骤4：判断得到的结果值是否大于约束条件的范围，否则执行步骤5，是则执行步骤6；

步骤5：基于

进行种群内各个体适应值计算，即计算运动轨迹的总时间，继续执行步骤7；

步骤6：将适应度赋值为比较大的数，该数值应远大于初始化时间种群的最大值，继续执行步骤7；

步骤7：判断是否满足终止条件，终止条件为迭代次数为固定次数最大值或者迭代中适应值收敛至不变。否则执行步骤8，是则执行步骤11；

步骤8：选择操作，根据选择操作经典轮盘赌选择法进行，即通过部分种群占总体的概率进行选择。具体的，本发明用时间最有作为适应值函数，在满足约束条件前提下，选择结果应是时间越短越优；

步骤9：交叉操作，在种群中随机选取部分个体，两个进行编组，进行二进制编码位置随机交换，产生一部分新的个体；

步骤10：变异操作，在种群中随机选取部分个体，进行个体数值随机变异操作，继续执行步骤2；

步骤11：输出符合约束条件的最优轨迹函数及种群个体。

实施例2

本发明实施例中提供了一种可避障的挖掘机工作装置作业轨迹规划***，包括：

避障单元，生成挖掘机工作装置的运动避障路径点；

规划单元，基于所述运动避障路径点，进行挖掘机工作装置运动轨迹规划，生成最优路径；

控制单元，基于所述最优路径，控制挖掘机工作装置的运动。

在本发明实施例的一种具体实施方式中，所述运动避障路径点的生成方法包括：

获得障碍物的位置信息以及形状信息；

基于挖掘机工作装置的运动起始点和运动目标点的坐标信息，以及所述障碍物的位置信息以及形状信息，规划出避障路径点；

在具体实施过程中，为了采集障碍物的位置信息以及形状信息，需要进行传感器的布置，可以采用如图2所示的布置方式：

1)挖掘机正前方安装：2台激光雷达1、1台长距毫米波雷达2；

2)挖掘机车体两侧对称安装：各1台激光雷达1；

3)挖掘机后方安装：2台激光雷达1、1台长距毫米波雷达2；

4)驾驶室顶部安装：1台激光雷达1；

5)挖掘机车内部安装：1台交换机、1台工控机3；

6)挖掘机臂安装：1台激光雷达1；

7)挖掘机工作装置安装：倾角传感器4、长度传感器5；所述挖掘机工作装置包括：动臂、斗杆和铲斗；

8)挖掘机上车安装惯性导航***(采用的是GPS坐标系)及控制器6、GPS天线7。

所述基于挖掘机工作装置的运动起始点和运动目标点的坐标信息，以及所述障碍物的位置信息以及形状信息，规划出避障路径点，具体为：

根据障碍物的位置信息以及形状信息，设置障碍物的栅格位置以及面积；

根据挖机工作装置末端的运动起始点和运动目标点的坐标信息，将挖掘工作装置运动空间栅格化后，取栅格中心为节点；

利用A*算法，基于障碍物的栅格位置、面积，以及栅格化后的挖掘工作装置运动空间，根据节点的优先级，进行避障的路径规划，所述A*算法通过以下函数来计算每个节点的优先级：

F(n)＝G(n)+H(n)

其中，F(n)是节点n的综合优先级，G(n)是节点n距离起点的代价，H(n)是节点n距离终点的预计代价，当选择下一个要遍历的节点时，总会选取综合优先级值最小的节点。

A*算法的流程图见图4，具体步骤如下：

步骤1：初始化open列表和close列表为空，将起点加入到open列表中；open列表如果为空，那么结束，否则继续；

步骤2：计算open列表中每个节点的F值，F＝G+H，H的计算使用Manhattan方法，找出open列表中F值最小的节点n，同时根据F值更新每个节点的父节点；

步骤3：将节点n从open列表中移除，添加到close列表中；

步骤4：节点n如果是目标节点，那么得到路径，从终点回溯到起点，否则继续；

步骤5：节点n如果没有子节点，那么返回步骤2，否则继续；

步骤6：查找节点n相邻的节点，跳过close列表中的节点以及不可到达的节点，将这些相邻的节点加入到open列表中，返回步骤2。

步骤7：open列表中的节点组成了可行的路径点,然后将每个路径点发送给路径规划模型，该路径点就对应每个栅格的中心。

在本发明实施例的一种具体实施方式中，所述最优路径的生成方法包括：

以所述运动避障路径点为约束条件，以挖掘机工作装置各关节的角度变化量之和最小为目标，筛选出最优的路径点；

基于所述最优的路径点，利用插值函数分段建立三次多项式；

以动作时间最短为目标函数，以运动速度、加速度、脉动曲线不超过相应约束范围为约束条件，求解出三次多项式的各项系数，得到最优路径。

所述三次多项式为：

P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0

其中，P_ji(t)表示的是第j个关节的第i段轨迹表达式，在N个关节的机械臂中j＝1,2,3,…,N，当挖掘机工作装置从起始点至目标点共计有n个中间点时，每个关节的轨迹可以划分为第i＝1,2,3,…,n-1段；s_ji3表示的是第j个关节、第i段轨迹中三次项的系数，s_ji2表示的是第j个关节、第i段轨迹中二次项的系数，s_ji1表示的是第j个关节、第i段轨迹中一次项的系数，s_ji0表示的是j第个关节、第i段轨迹中常数项的系数；

所述目标函数为：

所述约束条件为：

其中，g为工作装置运动所用的总时间，P_j'_i(t)，P_j”_i(t)，P_j”_i'(t)分别表示轨迹函数P_ji(t)的一，二，三阶导数，V_j,max为第j个关节的速度约束条件，W_j,max为第j个关节的加速度约束条件，K_j,max为第j个关节的脉动约束条件，h_j,i＝t_j,i+1-t_j,i为第j个关节在第i段轨迹所需要的时间。

在本发明实施例的一种具体实施方式中，所述最优的路径点的筛选方法具体为：

对挖掘机工作装置所处坐标系进行旋转和平移变换，获得第i个坐标系相对于车身坐标系的齐次变换矩阵T_i ⁰；

利用逆运动学方法，根据挖掘机工作装置末端的位置，求取挖机工作装置中各关节变量，其中，在逆运动学方法中，目标函数min(|α_i-α_i-1|+|β_i-β_i-1|+|θ_i-θ_i-1|)，其中α_i，β_i，θ_i分别表示三个关节在第i段轨迹运动的角度，所述挖掘机工作装置末端的位置是基于将GPS坐标系下的数据转换成当地水平坐标系中的数据。

所述齐次变换矩阵T_i ⁰的计算公式为：

具体地，所述最优路径的生成过程为：

1)挖掘机工作装置位姿与运动描述：

挖掘机工作装置的坐标系见图3，其中坐标系的建立符合右手定则，坐标系建立后，可以通过4个步骤将坐标系o_i-1x_i-1y_i-1z_i-1变换为下一个坐标系o_ix_iy_iz_i：

步骤1：绕z_i-1轴旋转θ_i度，将x_i-1和x_i轴变为相同方向，此变换矩阵记作Rot(Z,θ_i)，Z表示围绕Z轴进行旋转；

步骤2：沿z_i-1轴平移d_i的单位，使x_i-1和x_i轴共线，此变换矩阵记作Trans(0,0,d_i)；

步骤3：沿x_i-1轴平移a_i的单位，使两个坐标系的原点重合，此变换矩阵记作Trans(α_i,0,0)；

步骤4：绕x_i轴旋转a_i度，使z_i-1和z_i轴共线，两个坐标系随之完全重合，此变换矩阵记作Rot(X,α_i),X表示围绕X轴进行旋转；

第i-1个坐标系和第i个坐标系的关系可用矩阵

表示，其中：

根据平移矩阵和旋转齐次变换公式，可得：

因此

第i个坐标系相对于车身坐标系的齐次变换矩阵T_i ⁰可以用各连续坐标系齐次变换矩阵的乘积表示，即

在本发明实施例中，利用逆运动学方法，根据挖机工作装置末端(即铲斗齿尖末端)的位置，求取挖机工作装置各关节变量。由于挖机工作装置只有三个关节，因此可得：

根据上式，可得

其中，E为单位矩阵。

所述挖掘机工作装置末端的位置是基于将GPS坐标系下的数据转换成当地水平坐标系中的数据，具体转换过程为：

车辆位置与其周围障碍物位置采用绝对坐标系下的坐标位置--地球坐标系。由于通过GPS天线和惯导读到的GPS数据是地理坐标系(WGS84)，需要对该GPS数据进行解析与坐标变换，转换成以X轴指向正东，Y轴指向正北，原点位于惯性导航单元质心的直角坐标系数据；解析与坐标变换既可以通过高斯正算公式的计算实现。具体方法如下：

惯性导航单元可将采集到的GPS数据输出给工控机，GPS数据采用GPRMC格式(即最小定位信息)，包含了时间日期、经纬度、航向、磁偏角等信息；在***中创建GPS数据结构体，对得到的GPRMC数据进行数据解析，得到经度(L)和纬度(B)数据；由于采集到GPS数据是地球坐标，不能直接给挖掘机使用，需要通过高斯投影的方法，将L、B坐标通过高斯投影(高斯正算公式)得到高斯平面直角坐标，高斯正算公式可实现的精度为0.1米，具体公式如下所示：

X＝m₀+m₂l²+m₄l⁴

Y＝n₁l+n₃l³+n₅l⁵

式中l是经差，m₀,n₁,m₂,...是待定系数，他们是维度B的函数；(X,Y)为转换后的坐标。然后再将高斯平面直角坐标，转换成当地的水平坐标，则得到的坐标系y轴向东为正，x轴向北为正，与习惯的坐标系定义不统一，通过坐标转换可得到通用坐标系。转换公式如下：

通过上述方法将WGS84坐标系下的GPS数据转换到X轴指向正东，Y轴指向正北，原点位于传感器质心的当地水平坐标系中。

跟据转换后的车身GPS数据，且挖掘机工作装置各关节角度可通过传感器数据采集，通过机械臂正运动学，便可解算得到挖掘机工作装置末端位置及姿态。

在基于获得的路径点进行挖机工作装置运动逆解计算过程中，本发明的优化函数为各关节的角度变化量之和最小，即运动路最短，即min(|α_i-α_i-1|+|β_i-β_i-1|+|θ_i-θ_i-1|)，约束条件为前述的路径规划，其中α_i，β_i，θ_i分别表示三个关节在第i段轨迹运动的角度，然后采用粒子群优化算法进行优化求解，其中粒子群优化算法的流程图见图5。算法具体步骤如下：

步骤1：初始化一群微粒，包括随机位置与速度；

步骤2：评价每个微粒的适应度；

步骤3：对每个微粒，将其适应值与其经过的最好位置pbest(即：局部最优位置)作比较，如果较好，则将其作为当前的最好位置gbest(即：全局最优位置)；

步骤4：根据步骤2、步骤3式调整微粒速度与位置；

步骤5：未达到迭代终止条件则转步骤2继续。

迭代终止条件选为最大迭代次数或微粒群在最大迭代次数前搜索到的最优位置，即结果收敛，筛选出最优的路径点。

2)计算挖掘机工作装置的轨迹函数

三次多项式插值函数(即三次样条插值函数)是可以保证位置、速度、加速度连续的阶次最低多项式，因而成为使用广泛、计算简单的方法之一。因此釆用三次多项式样条函数方法在关节空间中对挖掘机运动轨迹进行规划，可以使受控变量与运动时间相关，轨迹规划时计算过程简单方便。

三次样条多项式轨迹的一般形式可以表示为：

P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0

其中，P_ji(t)表示的是第j个关节的第i段轨迹表达式，在N个关节的机械臂中j＝1,2,3,…,N，当机械臂从起始点至目标点共计有n个中间点(包含起始点与终止点)时，每个关节的轨迹可以划分为第i＝1,2,3,…,n-1段。s_ji3表示的是第j个关节、第i段轨迹中三次项的系数，s_ji2表示的是第j个关节、第i段轨迹中二次项的系数，s_ji1表示的是第j个关节、第i段轨迹中一次项的系数，s_ji0表示的是j第个关节、第i段轨迹中常数项的系数。由于末端执行器是各个关节共同作用的结果，因每个关节都有n-1段轨迹，所以末端执行器的运动路径也分为n-1段。

3)基于适应值函数的挖掘机工作装置轨迹规划

轨迹规划的最终目的就是求出P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0中各项的系数，并保证将所求出的各个系数代入P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0后所得的关节角度、速度、加速度、脉动曲线中，每一条曲线均能满足其相应的约束条件。

对挖掘机的工业应用而言，它的运动速度与生产效率密切相关，为了最大化提高挖掘机效率，本发明把它的动作时间优化至最短。即在轨迹函数表达式求解过程中，除了使各关节轨迹满足运动学约束条件之外，还要保证总的运动时间最短。此问题的实质就是如何合理地求解挖掘机工作装置工作时两个相邻节点间的时间间隔，从而使所有时间间隔的总和总运动时间最小的问题。除此之外，当用优化出的时间进行轨迹规划时，必须保证轨迹的速度、加速度、脉动曲线不能超过相应约束范围，否则，优化出的时间是无效的，必须重新进行轨迹的规划。

本发明将所有关节同时进行优化，此方法的特点是求解过程比较复杂，但是所有关节运动时间均相同，动作连续性好，控制简单。

对具有N个关节的挖机臂，进行如下定义：

V_j,max为第j个关节的速度约束条件，W_j,max为第j个关节的加速度约束条件，K_j,max为第j个关节的脉动约束条件。h_j,i＝t_j,i+1-t_j,i为第j个关节在第i段轨迹所需要的时间。

优化目标函数为：

约束条件为：

其中，g为工作装置运动所用的总时间，P_j'_i(t)，P_j”_i(t)，P_j”_i'(t)分别表示轨迹函数P_ji(t)的一，二，三阶导数，V_j,max为第j个关节的速度约束条件，W_j,max为第j个关节的加速度约束条件，K_j,max为第j个关节的脉动约束条件，h_j,i＝t_j,i+1-t_j,i为第j个关节在第i段轨迹所需要的时间。

将总时间作为优化的适应值函数，进行寻优求解。本发明利用遗传算法进行求解，见图6所示。如果遗传算法搜寻到的解满足约束条件，则计算适应值后继续进行迭代，如果遗传算法搜寻的解不满足约束条件，则将适应值设置为一个极大的数。

具体步骤如下：

步骤1：创建初始化某固定数量的时间种群，种群里的个体即为各关节运动时间；

步骤2：通过时间种群更新P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0各项系数；

步骤3：将时间种群中的时间间隔带入轨迹函数，计算轨迹函数相关约束条件的最大值；

步骤4：判断得到的结果值是否大于约束条件的范围，否则执行步骤5，是则执行步骤6；

步骤5：通过

进行种群内各个体适应值计算，即计算运动轨迹的总时间，继续执行步骤7；

步骤6：将适应度赋值为比较大的数，该数值应远大于初始化时间种群的最大值，继续执行步骤7；

步骤7：判断是否满足终止条件，终止条件为迭代次数为固定次数最大值或者迭代中适应值收敛至不变。否则执行步骤8，是则执行步骤11；

步骤8：选择操作，根据选择操作经典轮盘赌选择法进行，即通过部分种群占总体的概率进行选择。具体的，本发明用时间最有作为适应值函数，在满足约束条件前提下，选择结果应是时间越短越优；

步骤9：交叉操作，在种群中随机选取部分个体，两个进行编组，进行二进制编码位置随机交换，产生一部分新的个体；

步骤10：变异操作，在种群中随机选取部分个体，进行个体数值随机变异操作，继续执行步骤2；

步骤11：输出符合约束条件的最优轨迹函数及种群个体。

以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (10)

1.一种可避障的挖掘机工作装置作业轨迹规划方法，其特征在于，包括：

生成挖掘机工作装置的运动避障路径点；

基于所述运动避障路径点，进行挖掘机工作装置运动轨迹规划，生成最优路径；

基于所述最优路径，控制挖掘机工作装置的运动。

2.根据权利要求1所述的一种可避障的挖掘机工作装置作业轨迹规划方法，其特征在于，

所述运动避障路径点的生成方法包括：

获得障碍物的位置信息以及形状信息；

基于挖掘机工作装置的运动起始点和运动目标点的坐标信息，以及所述障碍物的位置信息以及形状信息，规划出避障路径点。

3.根据权利要求2所述的一种可避障的挖掘机工作装置作业轨迹规划方法，其特征在于，

所述基于挖掘机工作装置的运动起始点和运动目标点的坐标信息，以及所述障碍物的位置信息以及形状信息，规划出避障路径点，具体为：

根据障碍物的位置信息以及形状信息，设置障碍物的栅格位置以及面积；

根据挖机工作装置末端的运动起始点和运动目标点的坐标信息，将挖掘工作装置运动空间栅格化后，取栅格中心为节点；

利用A*算法，基于障碍物的栅格位置、面积，以及栅格化后的挖掘工作装置运动空间，根据节点的优先级，进行避障的路径规划。

4.根据权利要求3所述的一种可避障的挖掘机工作装置作业轨迹规划方法，其特征在于，

所述A*算法通过以下函数来计算每个节点的优先级：

F(n)＝G(n)+H(n)

其中，F(n)是节点n的综合优先级，G(n)是节点n距离起点的代价，H(n)是节点n距离终点的预计代价，当选择下一个要遍历的节点时，总会选取综合优先级值最小的节点。

5.根据权利要求1所述的一种可避障的挖掘机工作装置作业轨迹规划方法，其特征在于：

所述最优路径的生成方法包括：

基于所述运动避障路径点，利用插值函数分段建立三次多项式；

以动作时间最短为目标函数，以运动速度、加速度、脉动曲线不超过相应约束范围为约束条件，求解出三次多项式的各项系数，得到最优路径。

6.根据权利要求5所述的一种可避障的挖掘机工作装置作业轨迹规划方法，其特征在于：所述三次多项式为：

P_ji(t)＝s_ji3t³+s_ji2t²+s_ji1t+s_ji0

其中，P_ji(t)表示的是第j个关节的第i段轨迹表达式，在N个关节的机械臂中j＝1,2,3,…,N，当挖掘机工作装置从起始点至目标点共计有n个中间点时，每个关节的轨迹可以划分为第i＝1,2,3,…,n-1段；s_ji3表示的是第j个关节、第i段轨迹中三次项的系数，s_ji2表示的是第j个关节、第i段轨迹中二次项的系数，s_ji1表示的是第j个关节、第i段轨迹中一次项的系数，s_ji0表示的是j第个关节、第i段轨迹中常数项的系数；所述目标函数为：

所述约束条件为：

其中，g为工作装置运动所用的总时间，P'_ji(t)，P”_ji(t)，P”’_ji(t)分别表示轨迹函数P_ji(t)的一，二，三阶导数，V_j,max为第j个关节的速度约束条件，W_j,max为第j个关节的加速度约束条件，K_j,max为第j个关节的脉动约束条件，h_j,i＝t_j,i+1-t_j,i为第j个关节在第i段轨迹所需要的时间。

7.根据权利要求5所述的一种可避障的挖掘机工作装置作业轨迹规划方法，其特征在于：所述利用插值函数分段建立三次多项式步骤之前还包括：

以所述运动避障路径点为约束条件，以挖掘机工作装置各关节的角度变化量之和最小为目标，筛选出最优的路径点；

则所述三次多项式基于筛选出最优的路径点利用插值函数分段建立。

8.根据权利要求1所述的一种可避障的挖掘机工作装置作业轨迹规划方法，其特征在于，所述最优的路径点的筛选方法具体为：

对挖掘机工作装置所处坐标系进行旋转和平移变换，获得第i个坐标系相对于车身坐标系的齐次变换矩阵T_i ⁰；

利用逆运动学方法，根据挖掘机工作装置末端的位置，求取挖机工作装置中各关节变量，其中，在逆运动学方法中，目标函数min(|α_i-α_i-1|+|β_i-β_i-1|+|θ_i-θ_i-1|)，其中α_i，β_i，θ_i分别表示三个关节在第i段轨迹运动的角度；所述挖掘机工作装置末端的位置是基于将GPS坐标系下的数据转换成当地水平坐标系中的数据。

9.根据权利要求8所述的一种可避障的挖掘机工作装置作业轨迹规划方法，其特征在于：所述齐次变换矩阵T_i ⁰的计算公式为：

10.一种可避障的挖掘机工作装置作业轨迹规划装置，其特征在于，包括：

避障单元，生成挖掘机工作装置的运动避障路径点；

规划单元，基于所述运动避障路径点，进行挖掘机工作装置运动轨迹规划，生成最优路径；

控制单元，基于所述最优路径，控制挖掘机工作装置的运动。

Images