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

权利要求书

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₂表示末端在旋转过程中的轨迹交点，根据三个旋转轨迹的几何关系，矢量线段绕轴旋转时，其长度不变；旋转轨迹面垂直于旋转矢量，得到以下方程组，

设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′，q_e2的运动描述如下，

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

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

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

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

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上，否则，点在中心线段外；

设垂足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，以最小化运动时间为优化目标，则优化问题可描述如下，

f＝min{t_e}

式中θ_i,min，θ_i,max为各关节角位移极限值，和分别表示各关节角速度、角加速度的最大允许值，1≤i≤8；

步骤(6)优化轨迹；

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