CN109955250A - 应用于工业机器人的追踪与实时避障反应规划算法 - Google Patents

Abstract

本发明提供了一种应用于工业机器人的追踪与实时避障反应规划算法，包括：步骤1，输入待追踪曲线位姿、末端刚体与障碍物间的实时距离；步骤2，通过运动学正解给出从机器人关节空间到工作空间的映射；步骤3，通过视觉传感器检测障碍物并计算与障碍物距离；步骤4，规划路径，工作空间刚体追踪及避障；步骤5，根据关节空间中雅克比伪逆法求关节角；步骤6，利用快速搜索生成树处理角约束问题。步骤7，约束检测，如不满足约束，则重复步骤6；如满足约束，则进入步骤8。步骤8，达到目标，得到关节角并据此计算关节空间控制力矩；达到目标，则结束算法流程；否则进入步骤9；步骤9，更新初始点，重新从步骤3执行。

Description

应用于工业机器人的追踪与实时避障反应规划算法

技术领域

本发明涉及一种工业机器人路径规划与运动控制技术，特别是一种应用于工业机器人的追踪与实时避障应规划算法。

背景技术

在工程上，人们要求机器人能以最快或最小能量消耗的方式去完成特定的任务，因此经常使用优化的方法处理机器人的运动规划问题。但是，机器人运动方程的复杂性会导致相应优化问题的求解非常困难甚至不可解。利用李群和李代数的理论，可以给出复杂机器人运动方程等价的简单描述。更重要的是，可以精确计算力矩关于机器人任一关节变量的一阶和二阶导数。这样在求解最小能量消耗问题时，可以求得对应优化问题目标泛函的梯度和Hessian矩阵的精确表达式，从而避免了用数值近似的手段来处理梯度和Hessian矩阵所带来的昂贵成本、精确性差以及收敛上的不稳定性。另一方面，利用李群给出机器人运动方程的迭代表述公式时(复杂度为O(n))，可以避免使用三角函数及反三角函数所带来的关节角的奇异性问题，并极有效地降低计算复杂度。

考虑到工业机器人的工作环境中的障碍物，在处理机器人的避障问题时，常常采用物体间距离函数来检测障碍。当{遇到非光滑障碍物使}距离函数不可微时，在优化问题中获取梯度有实质性的困难。利用凸分析理论,将障碍约束转变为线性不等式组{作为优化目标的(非线性)约束}，可避免上述缺点。在检测障碍碰撞时，使用凸多面体划分，并采用“隐面剔除”策略，使得仅仅需要凸多面体的一小部分面来建立避障约束，从而在障碍物较多时，能极大地提高计算效率。

对于最省时间的最优控制问题，可以利用离散力学和最优控制(DiscreteMechanics andOptimal Control,DMOC)的框架来处理。其中的等式约束由本文提出的Hamel变分积分子得到。

发明内容

本发明的目的在于提供一种应用于工业机器人的追踪与实时避障反应规划算法，包括：

步骤1，输入待追踪曲线位姿、末端刚体与障碍物间的实时距离；

步骤2，通过运动学正解给出从机器人关节空间到工作空间的映射；

步骤3，通过视觉传感器检测障碍物并计算与障碍物距离；

步骤4，规划路径，工作空间刚体追踪及避障；

步骤5，根据关节空间中雅克比伪逆法求关节角；

步骤6，利用快速搜索生成树处理角约束问题。

步骤7，约束检测，如不满足约束，则重复步骤6；如满足约束，则进入步骤8。

步骤8，达到目标，得到关节角(θ₁,θ₂...,θ₆)并据此计算关节空间控制力矩；达到目标，则结束算法流程；否则进入步骤9；

步骤9，更新初始点，重新从步骤3执行。

本发明应用李群理论避免关节角的奇异性问题，并极有效地降低计算复杂度，采用“隐面剔除”策略在障碍物较多时，能极大地提高计算效率。

下面结合说明书附图对本发明作进一步描述。

附图说明

图1为凸多面体示意图。

具体实施方式

本算法目标：实现多自由度工业机器人路径规划的最优控制。

本算法输入信息：机器人的各项参数(包括关节角约束)，静态障碍物(凸多面体)信息，机器人的初始状态和终止状态及优化问题的初值，优化目标(最省能量或最省时间)。

本算法输出信息：满足约束和优化目标的各关节上的控制力矩及关节角。

本算法的技术思路：

利用序列二次规划SQP求解n连杆工业机器人的最优路径规划问题，应用机器人动力学的李群理论得到了机器人动力学的几何不变算法及相应的Hamel变分积分子，并采用“隐面剔除”策略实时对障碍约束进行简化。

结合上上述三种理论方法，将机器人避障的最优控制问题转化为一个非线性优化问题(最省能量的最优控制问题)，并得到其离散形式(最省能量的离散最优控制框架)。

针对推导得到的最省能量的离散最优控制框架，按以下步骤求解：

a.通过B样条对变量进行参数化；

b.赋初值，运用递推动力学求出控制力矩τ；

c.用控制力矩表示出目标泛函，加上约束，建立优化问题，并运用SQP相关技术对其求解。

本算法主要涉及以下技术方法：

1.最优控制算法

考虑n连杆机器人，向量q表示机器人关节角，向量v为关节角的速度，向量u表示作用在机器人连杆重心的力矩。机器人无避障最优化控制问题的定义：

a.目标泛函

b.约束

机器人运动方程：

q'＝v (2)

M(q)v'＝G(q,v)+F(q,u) (3)

边界条件：

q(t_0)＝q_0,v(t_0)＝0,q(t_f)＝q_f,v(t_f)＝0 (4)

边界约束：

u_min≤u≤u_max (5)

其中M(q)表示正定质量矩阵，G(q,v)表示广义科里奥利力，F(q,u)表示作用在关节处的力矩和重力。方程(3)是机器人动力学的拉格朗日形式，q₀,q_f分别表示初始状态和终点状态。

2.递推动力学

本方法是基于李群理论方法构造复杂多体系统的运动方程，该方法关键之处是迭代计算非线性动力学，计算精确的梯度矩阵和数值近似的Hessian矩阵。

给定n杆的串行链，令f_k-1,k∈SE(3)表示从第k-1个杆到第k个杆的变换矩阵，表示第k个关节的关节扭转，M_k∈SE(3)表示当q_k＝0时f_k-1,k的值，是第k个杆6维广义速度，F_k是第k-1个杆施加在第k个杆上的6维广义力，τ_k表示第k个关节所受力矩，J_k表示第k个杆的广义惯性矩阵，形式如下：

其中是第k个杆关于质心的旋转惯量，r_k是第k个杆框架原点到其质心的3维向量，m_k是第k杆的质量，I是3×3的单位阵，根据这些定义，给出递归方法计算逆动力学：

初始化：

向前迭代，k:＝1to n

向后迭代，k:＝n to 1

3.避障方法

为了建立避障准则，将机器人近似为一系列凸多面体的并，记为P

其中，n_p为P中多面体的个数，对于i＝1,...,n_P，p_i是P⁽ⁱ⁾的面的个数。在(9)中的每个不等式都描述了多面体的一个面。指标"(i)"表示P中第i个多面体。

相似地，障碍近似为凸多面体的集合，记为Q

其中，n_Q为Q中多面体的数量，对于j＝1,...,n_Q，q_j表示Q^(j)中面的数量。指标"(j)"表示Q中第j个多面体。如图1所示，其中P由5个多面体组成，Q由3个多面体组成。

分别使用凸多面体P和Q表示机器人与障碍物，其不碰撞当且仅当

多面体P⁽ⁱ⁾和Q^(j)的定义说明上述关系与下述线性不等式组不可解等价

根据泛函的Hahn—Banach凸集分离定理得到下述结论：不等式组(12)无解当且仅当存在向量使得

因而机器人P(t)和障碍Q不碰撞，当且仅当对于每一对多面体(P⁽ⁱ⁾,Q^(j)),i＝1,...,n_p,j＝1,...,n_Q,存在向量使得w^(i,j)(t)≥0时有

其中S⁽ⁱ⁾(t)∈SO(3)是对应于关节角的第i个多面体的旋转矩阵。因此，在最优控制中，关于避障的状态约束为对i＝1,...,n_p,j＝1,...,n_Q有

其中ε＞0为给定的距离参数，这些约束称为反碰撞约束。为了方便下面模型的描述，记

因此，结合上述三种理论方法，机器人避障的最优控制问题便可转化为一个非线性优化问题(描述A)，并得到其离散形式(描述B)。

描述A：最省能量的最优控制问题

a.目标泛函

b.约束

边界条件：

q(t_0)＝q_0,v(t_0)＝0,q(t_f)＝q_f,v(t_f)＝0 (19)

边界约束：

q_min≤q(t)≤q_max (20)

障碍物约束：

G_i,j(q(t))^Tw^(i,j)(t)＝0 (21)

(q(t))^Tw^(i,j)(t)≤ε,i＝1,...,n_p,j＝1,...,n_Q (22)

其中，τ(q(t))表示达到当前状态的力矩和重力,q_min和q_max分别表示关节角运动的下限和上限，v(t₀)和v(t_f)分别表示初始时刻和末端时刻的速度，n_p和n_Q分别表示机器人和障碍凸多面体表示中多面体的个数。

描述B：最省能量的离散最优控制框架

给定时间区间[0,t_f]，取离散网格节点为t_l＝lh,l＝0,...,t_n,其中时间步长为

对于n连杆机器人，为了得到连续光滑的关节路径，使用B样条对关节变量q(t)进行参数化，得到关节轨迹的形式为：

其中B_i(t)为B样条基函数，P＝(p₁,...,p_tn)，p_i∈Rⁿ为控制节点。

a.递推动力学

初始化：

向前迭代：

向后迭代：

b.目标泛函

c.约束

边界条件：

q(t₀,P)＝q₀,v(t₀,P)＝0,q(t_f,P)＝q_f,v(t_f,P)＝0 (31)

边界约束：

q_min≤q(t_l,P)≤q_max (32)

障碍物约束：

G_i,j(q(t_l,P))^Tw^(i,j)＝0 (33)

g_i,j(q(t_l,P))^Tw^(i,j)≤ε (34)

w^(i,j)≥0,i＝1,...,n_p,j＝1,...,n_Q,l＝1,...,t_n (35)

其中，P＝(p_1,...,p_{t_n})。

Claims (6)

1.一种应用于工业机器人的追踪与实时避障反应规划算法，其特征在于，包括：

步骤1，输入待追踪曲线位姿、末端刚体与障碍物间的实时距离；

步骤2，通过运动学正解给出从机器人关节空间到工作空间的映射；

步骤3，通过视觉传感器检测障碍物并计算与障碍物距离；

步骤4，规划路径，工作空间刚体追踪及避障；

步骤5，根据关节空间中雅克比伪逆法求关节角；

步骤6，利用快速搜索生成树处理角约束问题。

步骤7，约束检测，如不满足约束，则重复步骤6；如满足约束，则进入步骤8。

步骤8，达到目标，得到关节角(θ₁,θ₂...,θ₆)并据此计算关节空间控制力矩；达到目标，则结束算法流程；否则进入步骤9；

步骤9，更新初始点，重新从步骤3执行。

2.根据权利要求1所述的方法，其特征在于，步骤4的具体过程为：

步骤401，设刚体位置x＝(x,y,z)，位姿矩阵R，刚体角速度为ω，质量为m，惯性矩阵为J_x,J_y,J_z，惯性张量J＝diag(J_x,J_y,J_z)，状态空间Q；

步骤402，假设控制输入u，定义力矩τ，与作用力f，外力矩记为τ_ext，得运动方程：

其中

τ_ext(s)为外力矩，τ(s,u)为施加输入u后的力矩，f_ext(s)为外作用力，f(s,u)为施加输入u后的作用力，s为自定义空间状态集合，为刚体运动加速度；ω_x、ω_y、ω_z分别为x、y、z轴的角速度分量；

步骤403，记刚体区域A(R,x)，区域A₁,A₂的欧式距离为prox(A₁,A₂)，若刚体不与障碍物O₁,..,O_l发生碰撞，则用PQP方法计算prox；

步骤404，假设在参考轨迹x_r的路径上突然遇到障碍，确定一个新的可达目标状态x_d为移动到目标点的位置，为移动到目标点的速度；若x_d＝x_r(t_g)，t_g∈[0,T]，T为参考运动轨迹，使刚体偏离障碍并且朝着x_d移动；

步骤405，定义误差项Δx＝x-x_d，选取控制律为

其中，k_x,k_v＞0，Γ(s)是斜对称矩阵，为刚体当前运动速度；

步骤406，令

其中，给定值k_g、v_max＞0，刚体到最近障碍物的距离记为d(R,x；O_i)＞0，为由刚体质心指向最近障碍的单位向量

步骤407，为了使目标位置状态可达，定义势能函数对其求导并带入运动方程(1)得

根据LaSalle不变性原理，选择使得保证x_d渐近可达；

步骤408，为了使目标姿态状态(R_d,ω_d)可达，选取对姿态的力矩控制律

其中，skew(A):＝(A-A^T)，算子是帽子映射的逆映射，K_R、K_w是正定矩阵，w_d为刚体期望体角速度。

3.根据权利要求1所述的方法，其特征在于，步骤5的具体过程为：

步骤501，令关节角的求解多项式为

其中，τ为关节空间控制力矩，f(θ)由关节空间到工作空间的映射，θ为关节角；

步骤502，对微分运动学方程进行式(11)的处理

Δθ＝J^T(JJ^T+λ²I)^-1Δe (11)

其中，为J的伪逆，J为雅克比矩阵，误差项x_*为期望目标，λ是非零的衰减常数。

4.根据权利要求1所述的方法，其特征在于，步骤6的具体步骤为：

设定生成树以概率p_g向目标点生长，在均匀采样得到采样点，在已有生成树中选择距离采样点最近的树节点作为父节点，记为tree_i，父节点tree_i与采样点之间距离d满足d≤pstep，其中pstep为某一设定的步长；若d＞pstep，则从父节点到采样点的方向上选择新的采样点；

采样点部分则在父节点的旋转矩阵邻域均匀采样产生；

采用雅克比伪逆法追踪采样点，若所得关节角满足角度约束及碰撞约束，则将采样点添加到生成树中。否则，重复进行上述步骤，直至到达目标点。

5.根据权利要求4所述的方法，其特征在于，旋转空间采样方法如下：

生成位姿矩阵的一个随机元素

随机选择3个点，u₁,u₂,u₃～[0,1]均匀分布，可以确定一个均匀、随机四元数q＝(q_r,q_i,q_j,q_k)如下：

从而得到旋转矩阵R∈SO(3)：

6.根据权利要求1所述的方法，其特征在于，步骤8中采用式(14)求得关节空间控制力矩

其中，M(θ)为对称正定的惯性矩阵，包括作用于关节的哥氏力和离心力项，包括重力和作用于关节的其它力项。