CN113442140B - 一种基于Bezier寻优的笛卡尔空间避障规划方法 - Google Patents

Abstract

本发明涉及一种基于Bezier寻优的笛卡尔空间避障规划方法，包括如下步骤：确定避障运动的起点、终点和障碍物位置，并用障碍物模型简化障碍物；采用四阶Bezier曲线作为算法优化的目标轨迹模型，确定中间三个控制点位置；通过空间离散化计算点到障碍物表面的最短有向距离，建立类势图，搭建空间离散数据库；对适应度函数进行更新计算，采用粒子群算法迭代优化三个中间控制点位置；利用四阶Bezier曲线计算获得避障轨迹方程，结合实际控制采样周期，完成避障轨迹规划。本发明与传统避障规划方法相比简化了步骤，保证了机器人运动轨迹的平滑性，保证了速度加速度的连续，且不需要额外的平滑过滤算法，实现更加简便，并提高了机器人在有障碍环境运动时的安全性。

Description

一种基于Bezier寻优的笛卡尔空间避障规划方法

技术领域

本发明属于机器人技术领域，具体涉及一种基于Bezier寻优的笛卡尔空间避障规划方法。

背景技术

随着机器人任务愈加多样，机器人工作于有障碍环境也将成为常态，为保证任务顺利完成，考虑避障的轨迹规划技术是必须的，它对于提高工业机器人，尤其是协作机器人的环境适应性，拓展任务形式具有重要意义。

目前机器人的避障规划方法，从轨迹的作用空间来看，主要分为两类：一类为关节空间避障规划，另一类为笛卡尔空间避障规划。两种方法在提供安全路径的本质上没有区别，都是为了保证机器人沿轨迹运动时不碰撞障碍物，但是与关节空间避障相比，笛卡尔空间的避障规划由于是对机器人末端空间轨迹的直接规划，因此更加直观，且可规划的轨迹形态也更加复杂，能够应用于更加复杂的障碍环境，具有更好的适应性和拓展性。

对于笛卡尔空间避障规划方法，目前大多都需要结合现代智能技术和理论，或由二维移动避障理论拓展得到，例如：增点重规划法、栅格法、人工势场法、PRM等，其侧重点大多都在于如何获得末端无碰撞轨迹，在实际应用时仍存在不足。

增点重规划法虽然相对简单，计算效率高，但由于需要根据障碍物确定增点位置及个数，这一方面会增加方法计算量，另一方面也常会带来规划轨迹不连续问题。

栅格法、人工势场法等概率搜索的避障方法，除了对本体避障的考虑不足外，规划的轨迹缺乏平滑柔顺性，常需要进一步平滑算法的辅助，处理相对复杂。

大多笛卡尔空间避障规划方法都是基于对末端点轨迹的无碰撞规划，缺少对机器人运动过程中本体姿态的考虑，存在运动安全隐患。

发明内容

本发明的目的是提供一种基于Bezier寻优的笛卡尔空间避障规划方法。

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

一种基于Bezier寻优的笛卡尔空间避障规划方法，包括如下步骤：

S1：确定避障运动的起点坐标S和终点坐标E，确定障碍物位置，并用障碍物模型简化障碍物，

S2：采用四阶Bezier曲线作为算法优化的目标轨迹模型，确定中间三个控制点位置 P₁,P₂,P₃，以确定轨迹形状，

S3：在有限规划空间内，通过空间离散化，计算点到障碍物表面的最短有向距离，建立类势图，搭建空间离散数据库，完成空间点和障碍物最短距离的一一映射，

S4：完成模型简化和数据库搭建后，对适应度函数进行更新计算，采用粒子群算法迭代优化三个中间控制点位置，

S5：优化得到三个控制点坐标后，结合起点坐标S和终点坐标E，利用四阶Bezier曲线计算获得避障轨迹方程，结合实际控制采样周期，最终完成避障轨迹规划。

优选地，在S2中，所述的轨迹模型为：

式中：

P_i(i＝0,1,...,4)表示轨迹的控制点，

P₀和P₄为避障运动的起点坐标S和终点坐标E，

P₁,P₂,P₃为决定轨迹形状的中间三个控制点。

优选地，在S2中，所述的中间三个控制点位置P₁,P₂,P₃均为三维坐标：

优选地，在S3中，所述的点到障碍物的最短有向距离为：

d(x,y,z)＝V_sdf，

式中：

d(x,y,z)为空间轨迹点(x,y,z)到空间障碍物的综合最短距离。

进一步优选地，若环境中存在多个障碍物，则所述的点到障碍物表面的最短有向距离为点到每个障碍物表面最短距离的最小值。

优选地，在S4中，所述的适应度函数采用长度约束f_len、转角约束f_turn和碰撞安全约束f_safe的加权，所述的适应度函数为：

f＝ω₁f_safe+ω₂f_len+ω₃f_turn。

进一步优选地，所述的长度约束f_len、转角约束f_turn采用归一化计算，所述的碰撞安全约束f_safe采用两级判断检测法。

进一步优选地，所述的两级判断检测法包括如下步骤：

(1)在基于起点S、终点E和控制点P₁,P₂,P₃下进行Bezier轨迹计算，对轨迹进行离散化，

(2)通过数据库查询，得到所有轨迹点的SDF值中的最小值sdf_min，根据轨迹安全指标 f_pathSafe定义判断各轨迹点是否与障碍物产生了碰撞点，

当最小值sdf_min大于设定的安全阈值safe_th时，末端轨迹安全，则进行步骤(3)，

当最小值sdf_min小于等于设定的安全阈值safe_th时，当前轨迹不安全，会发生碰撞，则跳过步骤(3)，直接进行步骤(4)，

(3)对离散轨迹点利用机器人的逆运动学计算各关节角度，利用正运动学的转换矩阵获得各关节节点的位置，以节点坐标构建连杆模型，利用几何关系对连杆模型的自碰撞和与障碍物的外碰撞进行判断，若所有离散点均无碰撞发生时，则说明无碰撞，否则说明发生碰撞，

(4)若无碰撞，则碰撞安全指标f_coll的值为0，碰撞安全约束f_safe＝0，若发生碰撞，则碰撞安全指标f_coll的值为1，碰撞安全约束f_safe＝f_pathSafe·f_coll。

进一步优选地，所述的轨迹安全指标f_pathSafe为：

优选地，在S4中，所述的粒子群算法的核心更新公式为：

v_ij(t+1)＝ωv_ij(t)+c₁R₁(p_ij(t)x_ij(t))+c₂R₂(p_gj(t)-x_ij(t))，

x_ij(t+1)＝x_ij(t)+αv_ij(t+1)，

式中：

ω为惯性权重因子，

c₁为个体学习因子，

c₂为全局学习因子，

R₁和R₂为0到1间的随机数，

x_ij和v_ij分别为粒子i的第j维度的位置分量和速度分量，

t为算法的当前迭代。

由于上述技术方案运用，本发明与现有技术相比具有下列优点：

1.本发明通过采用基于四阶Bezier曲线的避障规划轨迹，与传统避障规划方法相比简化了实现步骤，充分保证了机器人运动轨迹的平滑性，保证了速度加速度的连续，且不需要额外的平滑过滤算法，实现更加简便。

2.本发明全面考虑了轨迹和机器人本体的全局无碰撞，提高了机器人在有障碍环境运动时的安全性。

3.本发明通过采用搭建离散势图的判断方法，与每次的在线计算相比，大幅减小了计算的复杂度，防止了时间性能的浪费，提高迭代性能。

附图说明

附图1为本实施例的避障规划方法流程图；

附图2为本实施例的机器人简化连杆模型；

附图3为本实施例的AABB障碍物简化模型；

附图4为本实施例的空间离散化示意图；

附图5为本实施例的SDF数据库整体示意图；

附图6为本实施例的SDF数据库剖面示意图；

附图7为本实施例的两级判断检测法流程图；

附图8为本实施例的避障轨迹结果示意图；

附图9为本实施例的避障规划结果仿真效果示意图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

一种基于Bezier寻优的笛卡尔空间避障规划方法，以选取某协作七轴机器人，对其应用如图1所示，包括如下步骤：

S1：

确定避障运动的起点坐标S和终点坐标E，确定障碍物位置；

对机器人采用连杆模型简化，如图2所示，对障碍物采用AABB模型简化，如图3所示，可方便后续采用几何法对机器人和障碍物间进行碰撞检测。

S2：

采用四阶Bezier曲线作为算法优化的目标轨迹模型，Bezier曲线是应用于二维图形应用程序的数学曲线，Bezier曲线包括起始点、终止点、控制点，通过调整控制点，Bezier曲线的形状可发生变化。采用四阶Bezier曲线作为算法优化的目标轨迹模型可保证路径平滑，避免速度、加速度的突变，且四阶轨迹的灵活性也保证覆盖了绝大多数可行轨迹，轨迹模型的方程形式为：

式中：

P_i(i＝0,1,...,4)表示轨迹的控制点，

P₀和P₄为避障运动的起点坐标S和终点坐标E，

P₁,P₂,P₃为决定轨迹形状的中间三个控制点；

确定中间三个控制点位置P₁,P₂,P₃，以确定轨迹形状，由于中间三个控制点位置P₁,P₂,P₃均为三维坐标，因此整体优化对象是一个3×3维度的粒子，具体而言：

S3：

在有限规划空间内，将空间点进行精密离散化，如图4所示；

计算所有离散点到障碍物表面的最短有向距离，建立SDF势图，搭建空间离散数据库，完成空间点和障碍物最短距离的一一映射，具体而言，点到障碍物表面的最短有向距离为：

d(x,y,z)＝V_sdf，

式中：

d(x,y,z)为空间轨迹点(x,y,z)到空间障碍物的综合最短距离，

若环境中存在多个障碍物，则点到障碍物表面的最短有向距离为点到每个障碍物表面最短距离的最小值。

在本实施例中：

对于AABB模型障碍物，假设障碍物的最小坐标值为Obs_min＝(x_min,y_min,z_min)，最大坐标值为Obs_max＝(x_max,y_max,z_max)，则点P(x,y,z)到障碍物表面的最短有向距离的计算方法为：

(1)确定障碍物中心点坐标Obs_center(x_c,y_c,z_c)，具体而言：

(2)得到障碍物长宽高的一半值，具体而言：

(3)计算点到障碍物表面的最短有向距离，

当点在障碍物内时为负值，可取-1，

当点在障碍物外时为正值，具体而言：

完成点到障碍物表面的最短有向距离计算后，建立对应环境下的空间SDF数据库，如图 5和图6所示。

S4：

完成模型简化和数据库搭建后，对适应度函数进行更新计算，适应度函数采用长度约束 f_len、转角约束f_turn和碰撞安全约束f_safe的加权，具体而言：

f＝ω₁f_safe+ω₂f_len+ω₃f_turn，

其中：长度约束f_len、转角约束f_turn采用归一化计算，碰撞安全约束f_safe采用两级判断检测法，如图7所示，两级判断检测法包括如下步骤：

(1)在基于起点S、终点E和控制点P₁,P₂,P₃下进行Bezier轨迹计算，对轨迹进行离散化，

(2)通过SDF数据库查询，得到所有轨迹点的SDF值中的最小值sdf_min，避免了执行过程中轨迹上点每次的在线计算，造成时间性能的浪费，影响工作效率，

根据轨迹安全指标f_pathSafe定义判断各轨迹点是否与障碍物产生了碰撞点，轨迹安全指标f_pathSafe具体为：

当最小值sdf_min大于设定的安全阈值safe_th时，末端轨迹安全，则进行步骤(3)，

当最小值sdf_min小于等于设定的安全阈值safe_th时，当前轨迹不安全，会发生碰撞，则跳过步骤(3)，直接进行步骤(4)，

(3)对离散轨迹点利用机器人的逆运动学计算各关节角度，利用正运动学的转换矩阵获得各关节节点的位置，以节点坐标构建连杆模型，利用几何关系对连杆模型的自碰撞和与障碍物的外碰撞进行判断，若所有离散点均无碰撞发生时，则说明无碰撞，否则说明发生碰撞，

(4)若无碰撞，则碰撞安全指标f_coll的值为0，碰撞安全约束f_safe＝0，若发生碰撞，则碰撞安全指标f_coll的值为1，碰撞安全约束f_safe＝f_pathSafe·f_coll；

完成上述计算后，采用粒子群算法对四阶Bezier曲线的三个中间控制点进行搜索优化，粒子群算法即典型的智能仿生算法，算法执行前需要设置粒子群规模、最大迭代次数等，粒子群算法的核心更新公式为：

v_ij(t+1)＝ωv_ij(t)+c₁R₁(p_ij(t)x_ij(t))+c₂R₂(p_gj(t)x_ij(t))，

x_ij(t+1)＝x_ij(t)+αv_ij(t+1)，

式中：

ω为惯性权重因子，

c₁为个体学习因子，

c₂为全局学习因子，

R₁和R₂为0到1间的随机数，

x_ij和v_ij分别为粒子i的第j维度的位置分量和速度分量，

t为算法的当前迭代。

S5：

优化得到中间三个控制点坐标后，结合起点坐标S和终点坐标E，利用四阶Bezier曲线计算获得避障轨迹方程。

在实际使用时，结合设定的控制采样周期，最终完成避障轨迹规划，如图8和图9所示。

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

Claims (7)

1.一种基于Bezier寻优的笛卡尔空间避障规划方法，其特征在于：包括如下步骤：

S1：确定避障运动的起点坐标S和终点坐标E，确定障碍物位置，并用障碍物模型简化障碍物，

S2：采用四阶Bezier曲线作为算法优化的目标轨迹模型，确定中间三个控制点位置P₁,P₂,P₃，以确定轨迹形状，

S3：在有限规划空间内，通过空间离散化，计算点到障碍物表面的最短有向距离，建立类势图，搭建空间离散数据库，完成空间点和障碍物最短距离的一一映射，

S4：完成模型简化和数据库搭建后，对适应度函数进行更新计算，采用粒子群算法迭代优化三个中间控制点位置，所述的适应度函数采用长度约束f_len、转角约束f_turn和碰撞安全约束f_safe的加权，所述的适应度函数为：

f＝ω₁·f_safe+ω₂·f_len+ω₃·f_turn，

所述的长度约束f_len、转角约束f_turn采用归一化计算，所述的碰撞安全约束f_safe采用两级判断检测法，所述的两级判断检测法包括如下步骤：

(1)在基于起点坐标S、终点坐标E和控制点位置P₁,P₂,P₃下进行Bezier轨迹计算，对轨迹进行离散化，

(2)通过数据库查询，得到所有轨迹点的SDF值中的最小值sdf_min，根据轨迹安全指标f_pathSafe定义判断各轨迹点是否与障碍物产生了碰撞点，

当最小值sdf_min大于设定的安全阈值safe_th时，末端轨迹安全，则进行步骤(3)，

当最小值sdf_min小于等于设定的安全阈值safe_th时，当前轨迹不安全，会发生碰撞，则跳过步骤(3)，直接进行步骤(4)，

(3)对离散轨迹点利用机器人的逆运动学计算各关节角度，利用正运动学的转换矩阵获得各关节节点的位置，以节点坐标构建连杆模型，利用几何关系对连杆模型的自碰撞和与障碍物的外碰撞进行判断，若所有离散点均无碰撞发生时，则说明无碰撞，否则说明发生碰撞，

(4)若无碰撞，则碰撞安全指标f_coll的值为0，碰撞安全约束f_safe＝0，若发生碰撞，则碰撞安全指标f_coll的值为1，碰撞安全约束f_safe＝f_pathSafe·f_coll，

S5：优化得到三个控制点坐标后，结合起点坐标S和终点坐标E，利用四阶Bezier曲线计算获得避障轨迹方程，结合实际控制采样周期，最终完成避障轨迹规划。

2.根据权利要求1所述的基于Bezier寻优的笛卡尔空间避障规划方法，其特征在于：在S2中，所述的轨迹模型为：

式中：

P_i(i＝0,1,...,4)表示轨迹的控制点，

P₀和P₄为避障运动的起点坐标S和终点坐标E，

P₁,P₂,P₃为决定轨迹形状的中间三个控制点。

3.根据权利要求1所述的基于Bezier寻优的笛卡尔空间避障规划方法，其特征在于：在S2中，所述的中间三个控制点位置P₁,P₂,P₃均为三维坐标：

4.根据权利要求1所述的基于Bezier寻优的笛卡尔空间避障规划方法，其特征在于：在S3中，所述的点到障碍物表面的最短有向距离为：

d(x,y,z)＝V_sdf，

式中：

d(x,y,z)为空间轨迹点(x,y,z)到空间障碍物的综合最短距离。

5.根据权利要求4所述的基于Bezier寻优的笛卡尔空间避障规划方法，其特征在于：若环境中存在多个障碍物，则所述的点到障碍物表面的最短有向距离为点到每个障碍物表面最短距离的最小值。

6.根据权利要求1所述的基于Bezier寻优的笛卡尔空间避障规划方法，其特征在于：所述的轨迹安全指标f_pathSafe为：

7.根据权利要求1所述的基于Bezier寻优的笛卡尔空间避障规划方法，其特征在于：在S4中，所述的粒子群算法的核心更新公式为：

v_ij(t+1)＝ωv_ij(t)+c₁R₁(p_ij(t)-x_ij(t))+c₂R₂(p_gj(t)-x_ij(t))，

x_ij(t+1)＝x_ij(t)+αv_ij(t+1)，

式中：

ω为惯性权重因子，

c₁为个体学习因子，

c₂为全局学习因子，

R₁和R₂为0到1间的随机数，

x_ij和v_ij分别为粒子i的第j维度的位置分量和速度分量，

t为算法的当前迭代。

Images