CN109968358B

CN109968358B - 一种考虑运动平稳性的冗余机器人全关节避障轨迹优化方法

Info

Publication number: CN109968358B
Application number: CN201910242408.2A
Authority: CN
Inventors: 刘志峰; 许静静; 裴艳虎; 赵永胜
Original assignee: Beijing University of Technology
Current assignee: Beijing University of Technology
Priority date: 2019-03-28
Filing date: 2019-03-28
Publication date: 2021-04-09
Anticipated expiration: 2039-03-28
Also published as: CN109968358A

Abstract

本发明公开了一种考虑运动平稳性的冗余机器人全关节避障轨迹优化方法，更具体是采用五次多项式在关节空间进行插值，以得到连续且光滑的关节运动速度与加速度曲线，保证机器人实际操作的运动平稳性。同时，以球形包络障碍物为例，建立了机器人与障碍间的最小距离模型。最后建立以运动学及避障指标为约束条件，以运动时间最小为目标，以冗余关节角位移及时间为变量，建立了冗余机器人避障轨迹优化模型，为工业机器人在复杂环境中的安全平稳操作控制提供了一定的方法基础。

Description

一种考虑运动平稳性的冗余机器人全关节避障轨迹优化方法

技术领域

本发明属于冗余机器人运动控制研究领域，涉及一种考虑运动平稳性的冗余机器人全关节避障轨迹优化方法，该方法同时考虑了运动平稳性、运动效率及可靠避障的任务要求，为冗余机器人在障碍空间内的运动控制提供一定的方法基础。

背景技术

冗余机器人作为一种关节空间维数大于任务空间维数的机器人，具有较高的运动灵活性，能够在保证运动性能的情况下满足其他特定的需求，如避障、避免奇异位形、优化关节力矩等。因此，冗余机器人在工业生产中的应用需求越来越广泛。面对复杂的障碍环境，机器人避障轨迹规划已经成为国内外学者关注的研究热点之一。避障轨迹规划是指在给定的环境条件下，规划一条由起始位置指向目标位置的无障碍路径，保证机器人运动的安全性与可靠性。目前，机器人避障的方法主要自由空间法和人工势场法两种，前者主要包括建立C空间、确定单元连通性、确定搜索方法及路径优化四个步骤，能够保证解的完备性，但在解决避障问题时存在以下问题：(1)由于需通过离散化求得C空间，为满足精度要求，所占内存量巨大；(2)当机器人关节较多时，会降低自由路径搜索效率。以上问题的存在导致自由空间法的应用受到了一定的限制。人工势场法包括建立障碍物几何模型、计算障碍物与机械臂的距离、定义势函数及求解关节驱动力四个步骤，与自由空间法相比，其对动态障碍环境的适用性较强，但仍然存在以下问题：(1)在障碍密集的情况下，由于多个吸引势函数和排斥势函数的共同作用，导致计算结果陷入局部最小，此时机器人运动将停止不前；(2)在求解距离时需简化障碍几何模型，而该简化过程会导致自由空间的浪费。此外，以上两种方法无法保证运动效率及运动平稳性。因此，提出一种冗余机器人轨迹规划方法，能够在可靠避障的同时提高运动效率并保证运动的平稳性，是本专利要解决的关键问题。

发明内容

本发明旨在提供一种考虑运动平稳性的冗余机器人轨迹优化方法。该方法的主要特点是通过采用五次多项式插值算法得到了连续且光滑的关节运动速度与加速度曲线，从而保证了运动平稳性，同时以运动学参数及避障指标为约束条件，以最小化运动时间为目标得到了优化轨迹，提高了运动效率。

本发明是采用以下技术手段实现的：一种考虑运动平稳性的冗余机器人全关节避障轨迹优化方法，该方法的实现过程如下，为冗余自由度关节角位移取初值，基于旋量理论建立冗余机器人逆解模型。

基于以上逆解模型，将机器人末端在操作空间的目标位姿映射到关节空间，得到各关节对应的最终角位移，采用五次多项式在各关节的初始角位移与最终角位移间进行插值运算，得到各关节角位移轨迹点列。

依据机器人几何结构，将其等效为由不同直径的圆柱体组成的整体，以各关节角位移轨迹点列为输入，基于旋量理论计算各圆柱体中心线端点的运动路径点位置。

以各圆柱体中心线端点位置与障碍位置矢量为参量，建立机器人与障碍间的最小距离模型，用于预估不同运动时刻的避障指标。

以冗余关节角位移、运动时间为变量，同时考虑运动学约束及障碍空间约束，以运动时间为优化目标，确定优化问题。

针对以上优化问题，采用优化算法(如粒子群算法、遗传算法等)得到优化轨迹。

本发明的特点在于提出的轨迹优化方法能够在可靠避障的同时，保证机器人运动平稳性并提高运动效率，为冗余机器人在障碍环境中的可靠避障控制提供一定的方法支撑。

附图说明

图1某八轴机器人机械臂简图；

图2末端绕关节3、4和5的旋转运动示意图；

图3绕关节6和7的旋转运动示意图；

图4运动时间的优化历程

图5优化轨迹示意图；

图6机器人与障碍间的最小距离随时间的变化曲线。

具体实施方式

以下结合附图和实施例对本发明进行详细说明。

一种考虑运动平稳性的冗余机器人全关节避障轨迹优化方法，本方法的实现步骤如下，

步骤(1)建立冗余机器人逆解模型；

如图1所示，由于八轴机器人具有两个冗余关节，为了建立逆解模型，首先为关节1和2的角位移θ₁和θ₂取初值，然后基于该初始条件，将该问题转化为六轴机器人的逆解问题。

a.求解θ₃、θ₄和θ₅；

取关节6、7和8的轴线交点为名义末端，初始位置矢量为 q_s＝[L₄ -L₁ -L₃ -L₅]，根据任务要求给定该末端的目标位置矢量为 q_e＝[x y z]，那么，基于旋量理论末端绕关节3、4和5的运动描述如下，

式中

表示关节i运动旋量的指标坐标形式，由关节i的初始单位旋转矢量ω_i及轴上一点的坐标q_i计算得到，其取值如式(2)和(3)所示。q_s′和q_e1′分别表示为q_s′＝[q_s 1]^T和

其中q_e′＝[q_e 1]^T.

在该旋转运动示意图(图2)中c₁和c₂表示末端在旋转过程中的轨迹交点，根据三个旋转轨迹的几何关系(矢量线段绕轴旋转时，其长度不变；旋转轨迹面垂直于旋转矢量)，得到以下方程组，

设c₁和c₂点所在位置矢量为c₁＝[x₁ y₁ z₁]和c₂＝[x₂ y₂ z₂]，通过联立方程组(4)，变量x₁,y₁,z₁,x₂,y₂和z₂求解如下，

式中A、B、A₁和B₁是与运动参数及目标位置矢量相关的常数，

A＝(L₂+L₃)/L₄；

B＝[x_e1 ²+(L₁+y_e1)²+(L₂-z_e1)²-L₂ ²-L₅ ²+L₄ ²+L₃ ²]/(2L₄)；A₁＝A²+1；

B₁＝2(AB-L₂)；C₁＝B²+L₂ ²-x_e1 ²-(L₁+y_e1)²-(L₂-z_e1)²。

则关节3、4和5的逆解问题转化为3个Paden-Kahan子问题1，

基于子问题1的求解方法，关节3、4和5的角位移(θ₃、θ₄和θ₅)求解如下，

b.求解θ₆和θ₇

关节6、7和8的旋转运动表示为，

式中

取一点q_s2＝[0 -L₁ -L₃ -L₅]，该点在关节轴8上，且在关节轴6和7 外，因此，绕关节6和7旋转至q_e2(q_e2′＝g₁q_s2′)的运动描述如下，

在该旋转运动示意图(图3)中c₃表示点q_s2绕关节6和7的旋转轨迹交点，根据几何知识得到如下方程组，

通过求解得到轨迹交点坐标矢量为

则绕关节6和7的运动分别表示如下，

同理，基于Paden-Kahan子问题1的求解方法，θ₆和θ₇求解如下，

c.求解θ₈

取关节轴8外一点q_s3＝[L₄ 0 -L₃ -L₅]，则该点绕轴8的旋转运动表示为，

式中

则基于Paden-Kahan子问题1，θ₈表示为，

θ₈＝atan2[L₁(z_e3+L₃+L₅)，L₁(y_e3+L₁)] (14)

步骤(2)采用五次多项式插值得到关节空间轨迹；

通过以上逆解模型，求解与末端目标位姿相对应的关节i的目标角位移θ_i。其次，基于五次多项式插值，关节i的角位移、角速度及角加速度表示如下，

为保证各关节角速度及角加速度的连续性，且在启停时关节不发生关节变量突变导致的关节振颤，得到以下方程组，

式中t_e表示运动时间。

通过求解可得参数a_1,i,a_2,i,a_3,i,a_4,i,a_5,i,a_6,i的值如下，

最后可保证各关节运动平稳性的关节角位移表示如下，

θ_i(t)＝6θ_it⁵/t_e ⁵-15θ_it⁴/t_e ⁴+10θ_it³/t_e ³ (18)

步骤(3)计算各圆柱体中心线端点的运动路径点位置；

根据八轴机器人简图(图1)，该机器人可等效为五个臂杆构成的机械结构，将臂杆i等效为半径为R_i(mm)的圆柱体，则根据各圆柱体中心线端点的初始位置，基于旋量理论其在t时刻的位置坐标计算如下，

其中J_i(t)和J_i+1(t)分别表示第i个圆柱体中心线端点的当前位置坐标，1≤i≤5。步骤(4)建立机器人与障碍间的最小距离模型；

避开球形障碍，其半径均设为R_o(mm)，中心位置坐标为O＝[x_o y_o z_o]，取障碍中心O在臂杆i中心线J_iJ_i+1上垂足为O′，取步骤(3)中计算得到的中心线端点J_i与J_i+1的当前位置坐标分别为J_i＝[x_J,i y_J,i z_J,i]和 J_i+1＝[x_J,i+1 y_J,i+1 z_J,i+1]，则直线J_iJ_i+1的参数方程表示如下，

式中参数0≤η≤1表示点在线段J_iJ_i+1上，否则，点在线段外。

设垂足O′的坐标位置为O′＝[x_o′ y_o′ z_o′]，通过以下方程求得，

当垂足落在中心线段J_iJ_i+1上，设O′＝[x_o′ y_o′ z_o′]，此时臂杆i与障碍间的最小距离表示为，

当垂足未落在中心线段J_iJ_i+1上，臂杆i与障碍间的最小距离表示为，

则机器人与障碍间的最小距离表示为，

d＝min[d_i|1≤i≤5] (24)

步骤(5)优化问题描述；

在该避障轨迹规划问题中，以冗余关节角位移θ₁，θ₂和运动时间t_e为优化变量，同时考虑运动学约束(各关节角位移、角速度及角加速度的可取范围) 及障碍空间约束即机器人与障碍间的最小距离d不应小于任务所要求的安全阈值d_t，以最小化运动时间为优化目标，则该优化问题可描述如下，

式中θ_i,min，θ_i,max为各关节角位移极限值，

和

分别表示各关节角速度、角加速度的最大允许值，1≤i≤8。

步骤(6)优化轨迹；

设定以下初始条件，采用粒子群算法搜索最优轨迹，以保证运动性能。

a.取机器人末端目标位姿如下，

b.球形障碍3个，其中心位置坐标分别为O₁＝[300 -500 -200]； O₂＝[500 -50050]和O₃＝[500 100 -50]，半径均为10mm。取安全阈值 d_t＝10mm。

c.机器人运动学参数及臂杆等效圆柱体半径取值如表1所示，

表1 8轴机器人相关参数

参数

L1

L2

L3

L4

L5

L6

R1～R3

R4

R5

值(mm)

300

300*tan15°

409-L2

55

364

30

25

20

10

d.优化约束条件中参数取值如表2所示，

表2优化约束条件参数

则基于以上初始条件，可得到运动时间的优化历程、优化轨迹(如图4、5 所示)，及各障碍与机器人的最小距离随运动时间的变化曲线(如图6所示)。表3为各关节变量优化结果。其次，优化运动时间为4.4416s，机器人与障碍物间的最小距离为20mm。

表3各关节变量优化结果

关节	1	2	3	4	5	6	7	8
									角位移(°)	-25.836	-48.37	68.485	-131.532	105.209	-108.979	3.667	35.519
最大角速度(°/s)	10.906	20.419	28.91	55.525	44.413	46.004	1.5479	14.994
									最大角加速度(°/s<sup>2</sup>)	7.5433	14.123	19.996	38.403	30.718	31.818	1.0706	10.37

Claims

1.一种考虑运动平稳性的冗余机器人全关节避障轨迹优化方法，为冗余自由度关节角位移取初值，基于旋量理论建立冗余机器人逆解模型；

基于以上逆解模型，将机器人末端在操作空间的目标位姿映射到关节空间，得到各关节对应的最终角位移，采用五次多项式在各关节的初始角位移与最终角位移间进行插值运算，得到各关节角位移轨迹点列；

依据机器人几何结构，将其等效为由不同直径的圆柱体组成的整体，以各关节角位移轨迹点列为输入，基于旋量理论计算各圆柱体中心线端点的运动路径点位置；

以各圆柱体中心线端点位置与障碍位置矢量为参量，建立机器人与障碍间的最小距离模型，用于预估不同运动时刻的避障指标；

以冗余关节角位移、运动时间为变量，同时考虑运动学约束及障碍空间约束，以运动时间为优化目标，确定优化问题；

其特征在于：具体实现步骤如下，

步骤(1)建立冗余机器人逆解模型；

由于八轴机器人具有两个冗余关节，为了建立逆解模型，首先为关节1和2的角位移θ₁和θ₂取初值，然后基于初始条件，将问题转化为六轴机器人的逆解问题；

a.求解θ₃、θ₄和θ₅；

取关节6、7和8的轴线交点为名义末端，初始位置矢量为q_s＝[L₄ -L₁ -L₃ -L₅]，根据任务要求给定末端的目标位置矢量为q_e＝[x y z]，那么，基于旋量理论末端绕关节3、4和5的运动描述如下，

式中

表示关节i运动旋量的指标坐标形式，由关节i的初始单位旋转矢量ω_i及轴上一点的坐标q_i计算得到，其取值如式(2)和(3)所示；q_s′和q_e1′分别表示为q_s′＝[q_s 1]^T和

其中q_e′＝[q_e 1]^T；

在旋转运动中c₁和c₂表示末端在旋转过程中的轨迹交点，根据三个旋转轨迹的几何关系，矢量线段绕轴旋转时，其长度不变；旋转轨迹面垂直于旋转矢量，得到以下方程组，

A＝(L₂+L₃)/L₄；

B₁＝2(AB-L₂)；C₁＝B²+L₂ ²-x_e1 ²-(L₁+y_e1)²-(L₂-z_e1)²；

则关节3、4和5的逆解问题转化为3个Paden-Kahan子问题1，

基于子问题1的求解方法，关节3、4和5的角位移θ₃、θ₄和θ₅求解如下，

b.求解θ₆和θ₇

关节6、7和8的旋转运动表示为，

式中

取一点q_s2＝[0 -L₁ -L₃ -L₅]，该点在关节轴8上，且在关节轴6和7外，因此，绕关节6和7旋转至q_e2，q_e2′＝g₁q_s2′，q_e2的运动描述如下，

在旋转运动中c₃表示点q_s2绕关节6和7的旋转轨迹交点，根据几何知识得到如下方程组，

通过求解得到轨迹交点坐标矢量为

则绕关节6和7的运动分别表示如下，

c.求解θ₈

取关节轴8外一点q_s3＝[L₄ 0 -L₃ -L₅]，则该点绕关节轴8的旋转运动表示为，

式中

则基于Paden-Kahan子问题1，θ₈表示为，

θ₈＝atan 2[L₁(z_e3+L₃+L₅)，L₁(y_e3+L₁)] (14)

步骤(2)采用五次多项式插值得到关节空间轨迹；

通过以上逆解模型，求解与末端目标位姿相对应的关节i的目标角位移θ_i；其次，基于五次多项式插值，关节i的角位移、角速度及角加速度表示如下，

式中t_e表示运动时间；

通过求解可得参数a_1,i,a_2,i,a_3,i,a_4,i,a_5,i,a_6,i的值如下，

最后可保证各关节运动平稳性的关节角位移表示如下，

θ_i(t)＝6θ_it⁵/t_e ⁵-15θ_it⁴/t_e ⁴+10θ_it³/t_e ³ (18)

步骤(3)计算各圆柱体中心线端点的运动路径点位置；

八轴机器人可等效为五个臂杆构成的机械结构，将臂杆i等效为半径为R_i(mm)的圆柱体，则根据各圆柱体中心线端点的初始位置，基于旋量理论其在t时刻的位置坐标计算如下，

其中J_i(t)和J_i+1(t)分别表示第i个圆柱体中心线端点的当前位置坐标，1≤i≤5；步骤(4)建立机器人与障碍间的最小距离模型；

避开球形障碍，半径均设为R_o(mm)，中心位置坐标为O＝[x_o y_o z_o]，取障碍中心O在臂杆i中心线段J_iJ_i+1上垂足为O′，取步骤(3)中计算得到的中心线端点J_i与J_i+1的当前位置坐标分别为J_i＝[x_J,i y_J,i z_J,i]和J_i+1＝[x_J,i+1 y_J,i+1 z_J,i+1]，则直线J_iJ_i+1的参数方程表示如下，

式中参数0≤η≤1表示点在中心线段J_iJ_i+1上，否则，点在中心线段外；

则机器人与障碍间的最小距离表示为，

d＝min[d_i|1≤i≤5] (24)

步骤(5)优化问题描述；

在避障轨迹规划问题中，以冗余关节角位移θ₁，θ₂和运动时间t_e为优化变量，同时考虑运动学约束及障碍空间约束即机器人与障碍间的最小距离d不应小于任务所要求的安全阈值d_t，以最小化运动时间为优化目标，则优化问题可描述如下，

f＝min{t_e}

式中θ_i,min，θ_i,max为各关节角位移极限值，

和

分别表示各关节角速度、角加速度的最大允许值，1≤i≤8；

步骤(6)优化轨迹；

设定初始条件，采用粒子群算法搜索最优轨迹，以保证运动性能。