CN108621162A - 一种机械臂运动规划方法 - Google Patents

Abstract

本发明公开一种机械臂运动规划方法，包括如下步骤：A、在机械臂处于初始状态时，测算出机械臂末端的实际位置和机械臂末端期望的初始位置，计算出机械臂末端的位置误差；B、构建机械臂笛卡尔速度方程，利用雅克比矩阵的伪逆结合机械臂笛卡尔速度方程求解机械臂关节速度。本发明的方法能大大节约计算量，并且计算结果准确，能够有效提高机械臂的执行精度。

Description

一种机械臂运动规划方法

技术领域

本发明属于机械人控制技术领域，具体为一种机械臂运动规划方法。

背景技术

冗余度机械臂是一种自由度大于任务空间所需最少自由度的末端能动机械装置，其运动任务包括焊接、油漆、组装、挖掘和绘图等，广泛应用于装备制造、产品加工、机器作业等国民经济生产活动中。冗余度机械臂的逆运动学问题是指已知机械臂末端位姿，确定机械臂的关节角问题。当冗余度机械臂末端的初始位置与期望轨迹的初始位置存在偏差时，该偏差在指定的轨迹跟踪任务执行过程中一直存在，导致机械臂执行精度下降；而容错型冗余度解析方案，就是对出现偏差的机械臂末端位置进行容错处理，使得机械臂在执行指定轨迹跟踪任务的同时能够快速减小初始偏差对机械臂执行精度的影响。

以往当冗余度机械臂末端的初始位置存在偏差时，需要将原系统转化为低阶控制系统，再对控制系统重新设计来消除偏差，过程较为繁琐。

发明内容

本发明的目的在于提供一种计算量小、用于机械臂末端位置控制的容错型冗余度机械臂运动规划方法，本方法能大大节约计算量，并且计算结果准确，能够有效提高机械臂的执行精度。

为达到上述目的，本发明的技术方案如下：

一种机械臂运动规划方法，包括如下步骤：

A、在机械臂处于初始状态时，测算出机械臂末端的实际位置和机械臂末端期望的初始位置，计算出机械臂末端的位置误差；

B、构建机械臂笛卡尔速度方程，利用雅克比矩阵的伪逆结合机械臂笛卡尔速度方程求解机械臂关节速度。

优选地机械臂运动规划方法，包括以下步骤：

A、在机械臂处于初始状态时，测量实际的关节角度，根据实际的关节角度和机械臂期望的关节角度，进行机械臂的正向运动学分析，计算机械臂末端的笛卡尔位置向量，其中包括到对应的机械臂末端期望的初始位置和机械臂末端实际的初始位置数据；

B、基于机械臂末端执行器期望的初始位置和实际的初始位置数据，计算期望的机械臂末端轨迹；

C、通过关节空间的关节角向量和雅克比矩阵获得出机械臂的笛卡尔速度方程；

D、通过机械臂末端的笛卡尔位置向量和期望的机械臂末端轨迹，计算出实际和期望的机械臂末端执行器轨迹之间的向量值误差，利用该误差优化步骤C得到的笛卡尔速度方程；

E、通过优化后的笛卡尔速度方程结合步骤C中雅克比矩阵的伪逆获得机械臂关节速度的计算公式，计算出机械臂关节速度数据；

F、循环执行步骤A-E迭代求解，得到最优的关节速度数据，即为机械臂运动规划的数据，发送至机械臂控制器控制机械臂执行给定任务。

优选地，所述的步骤A中，利用了如下的正向运动学方程式进行计算机械臂末端的笛卡尔位置向量：

r(t)＝f(θ(t)) (1)；

其中，r(t)为关节空间的关节角向量，θ(t)对应的机械臂末端的笛卡尔位置向量，f(·)为给定机械手结构和参数的可微非线性函数。

优选地，所述的步骤B中，机械臂末端轨迹方程如下：

其中，t为任务执行时间，T为任务周期时间，X₀为期望的初始位置的X坐标，Y₀为期望的初始位置的Y坐标。

优选地，所述的步骤C中，机械臂的笛卡尔速度方程为：

其中，表示关节速度向量，表示对应于的关节速度的机械臂末端笛卡尔速度，J(θ)表示机械臂的雅可比矩阵。

优选地，所述的雅可比矩阵的构建过程为：

对于具有n个自由度的串联结构的机械臂，笛卡尔空间运动速度矢量与关节坐标矢量之间的关系为：

其中i＝1,2,…6表示机械臂末端执行器分别沿X、Y和Z方向的平移线速度矢量和旋转角速度矢量的个数，j＝1,2,…n表示机械臂的关节个数；

6×n的雅克比矩阵如下：

优选地，所述的步骤D中，实际和期望的机械臂末端执行器轨迹之间的向量值偏误差计算公式为：

d(t)＝r(t)-r_d(t) (6)；

其中，d(t)为机械臂末端执行器轨迹之间的向量值偏误差，r(t)为关节空间的关节角向量，r_d(t)为期望的机械臂末端轨迹。

优选地，所述的步骤D中，利用实际和期望的机械臂末端执行器轨迹之间的向量值偏误差优化后的笛卡尔速度方程的过程为：

a、对实际和期望的机械臂末端执行器轨迹之间的向量值偏误差求导，求导公式为

其中，为实际和期望的机械臂末端轨迹之间的向量值偏差随时间的导数，d(t)为机械臂末端执行器轨迹之间的向量值偏误差；其中参数λ(t)＝λ₀[1-cos(2πt/T)]，λ₀是一个可任意设置的标量系数，T表示机械臂执行任务的周期；

b、结合步骤C得到的笛卡尔速度方程和实际和期望的机械臂末端执行器轨迹之间的向量值偏误差求导公式，得到优化后的笛卡尔速度方程：

其中，表示机械臂末端执行器优化后的速度矢量，为实际和期望的机械臂末端轨迹之间的向量值偏差随时间的导数。

优选地，所述的步骤E中，机械臂关节速度的计算公式为：

其中，为机械臂关节速度，J⁺＝J^T(JJ^T)^-1是雅克比矩阵J的伪逆，表示机械臂末端执行器优化后的速度矢量。

本发明只需要根据存在初始偏差时的机械臂末端位置来调整相应的参数，就可进行容错，然后根据调整的新参数，机械臂能够完成指定轨迹任务。并且只需要通过伪逆方案处理就可以进行容错，从而避免了重新设计的繁琐过程，大为节约了计算量，提高了控制精度。

具体实施方式

下面通过具体实施例对本发明进行详细说明。

实施例1

以四自由度为例，正向运动学方程的构建过程案例如下：

1、为简化机械臂结构，设四自由度机械臂的D-H参数如下：

四自由度机械臂的D-H参数

连杆i	关节角θi	扭转角αi	连杆长度ai	连杆偏移量di
					1	θ1	0	l1	0
2	θ2	0	l2	0
					3	θ3	0	l3	0
4	θ4	0	l4	0

2、通过D-H参数求得机械臂的连杆变换矩阵：

其中cθ₁表示cos(θ₁)，sθ_i表示sin(θ_i)，cθ₁₂表示cos(θ₁+θ₂)，sθ₁₂₃₄表示sin(θ₁+θ₂)。

3、实际的关节角度设置为θ＝[0,π/2,π/4,0]^T，连杆长度设置为l＝[1,0.8,0.7,0.5]^T，那么机械臂末端的笛卡尔坐标可以通过下面的公式计算得出为：

即得到4自由度机械臂的正向运动学方程r＝f(θ)，其中r＝[r_x,r_y]^T。

其他自由度的机械手，也是参照同样的方法构建正向运动学方程。

实施例2

同样以4自由度机械臂为例，雅可比矩阵构建如下：

对于具有n个自由度的串联结构的机械臂，笛卡尔空间运动速度矢量与关节坐标矢量之间的关系为：

其中i＝1,2,…6表示机械臂末端执行器分别沿X、Y和Z方向的平移线速度矢量和旋转角速度矢量的个数，j＝1,2,…n表示机械臂的关节个数。

6×n的雅克比矩阵如下：

由于本专利中的四自由度机械臂有4个关节，并且机械臂在二维平面中工作，因此涉及到的具体的雅克比矩阵为：

其中cθ₁表示cos(θ₁)，sθ_i表示sin(θ_i)，cθ₁₂表示cos(θ₁+θ₂)，sθ₁₂₃₄表示sin(θ₁+θ₂)。

实施例3

以4自由度机械臂为例，本发明的整个过程如下：

机械臂期望初始位置关节角度设置为[π/6,π/12,π/6,0]^T，该机械臂实际位置设置为[0,π/2,π/4,0]^T，四个连杆长度设置为l＝[1,0.8,0.7,0.5]^T。设定机械臂四个连杆长度为l＝[1,0.8,0.7,0.5]^T，任务周期T为10s，λ₀设置为5；

设定位置偏差时的参数配置λ(t)＝λ₀[1-cos(2πt/T)]，λ₀是一个可任意设置的标量系数，T表示机械臂执行任务的周期；

首先，将机械臂期望的关节角度θ_d(0)、实际的关节角度θ(0)分别代入下面的正向运动学方程式(1)中进行机械臂的正向运动学分析：

r(t)＝f(θ(t)) (1)；

可以得到对应的机械臂末端执行器期望的初始位置为

r_d(0)＝(-1.39282032302755,1.26602540378444)和实际的初始位置为

r(0)＝(-1.37578323617208,1.13661588123318)；

设期望跟踪的轨迹方程为：写成矩阵形式为

其次，通过对式(2)可以得到机械臂笛卡尔速度方程：

这里的雅克比矩阵J(θ)为：

J+(θ)为J(θ)的伪逆，其中cθ₁表示cos(θ₁)，sθ_i表示sin(θ_i)，cθ₁₂表示cos(θ₁+θ₂)，sθ₁₂₃₄表示sin(θ₁+θ₂)；

得到机械臂笛卡尔速度方程为：

写成矩阵形式为可以知道t＝0时机械臂期望的初始速度

其次，通过(3)式计算出末端执行器的位置误差：

d(t)＝r(t)-r_d(t) (6)；

得到初始位置误差为d(0)＝(0.0170370868554659,-0.129409522551260)；

然后，通过式(4)求得误差对时间的导数；

将λ:＝λ₀[1-cos(2πt/T)]代入(3)式，其中T＝10，λ₀＝5，得到

之后，通过式(5)得到优化后的机械臂末端的笛卡尔速度：

则机械臂末端的为

最后，通过式(6)求解关节速度：

其中，J⁺(θ)为J(θ)的伪逆，当t＝0时得到

通过循环以上步骤得到任务周期内容错型冗余度机械臂运动规划的解。

Claims (9)

1.一种机械臂运动规划方法，其特征在于，包括如下步骤：

A、在机械臂处于初始状态时，测算出机械臂末端的实际位置和机械臂末端期望的初始位置，计算出机械臂末端的位置误差；

B、构建机械臂笛卡尔速度方程，利用雅克比矩阵的伪逆结合机械臂笛卡尔速度方程求解机械臂关节速度。

2.根据权利要求1所述的容错型冗余度机械臂运动规划方法，其特征在于，包括以下步骤：

A、在机械臂处于初始状态时，测量实际的关节角度，根据实际的关节角度和机械臂期望的关节角度，进行机械臂的正向运动学分析，计算机械臂末端的笛卡尔位置向量，其中包括到对应的机械臂末端期望的初始位置和机械臂末端实际的初始位置数据；

B、基于机械臂末端执行器期望的初始位置和实际的初始位置数据，计算期望的机械臂末端轨迹；

C、通过关节空间的关节角向量和雅克比矩阵获得出机械臂的笛卡尔速度方程；

D、通过机械臂末端的笛卡尔位置向量和期望的机械臂末端轨迹，计算出实际和期望的机械臂末端执行器轨迹之间的向量值误差，利用该误差优化步骤C得到的笛卡尔速度方程；

E、通过优化后的笛卡尔速度方程结合步骤C中雅克比矩阵的伪逆获得机械臂关节速度的计算公式，计算出机械臂关节速度数据；

F、循环执行步骤A-E迭代求解，得到最优的关节速度数据，即为机械臂运动规划的数据，发送至机械臂控制器控制机械臂执行给定任务。

3.根据权利要求2所述的容错型冗余度机械臂运动规划方法，其特征在于：

所述的步骤A中，利用了如下的正向运动学方程式进行计算机械臂末端的笛卡尔位置向量：

r(t)＝f(θ(t)) (1)；

其中，r(t)为关节空间的关节角向量，θ(t)对应的机械臂末端的笛卡尔位置向量，f(·)为给定机械手结构和参数的可微非线性函数。

4.根据权利要求2或3所述的容错型冗余度机械臂运动规划方法，其特征在于：

所述的步骤B中，机械臂末端轨迹方程如下：

其中，t为任务执行时间，T为任务周期时间，X₀为期望的初始位置的X坐标，Y₀为期望的初始位置的Y坐标。

5.根据权利要求2所述的容错型冗余度机械臂运动规划方法，其特征在于：

所述的步骤C中，机械臂的笛卡尔速度方程为：

其中，表示关节速度向量，表示对应于的关节速度的机械臂末端笛卡尔速度，J(θ)表示机械臂的雅可比矩阵。

6.根据权利要求5所述的容错型冗余度机械臂运动规划方法，其特征在于：

所述的雅可比矩阵的构建过程为：

对于具有n个自由度的串联结构的机械臂，笛卡尔空间运动速度矢量与关节坐标矢量之间的关系为：

其中i＝1,2,…6表示机械臂末端执行器分别沿X、Y和Z方向的平移线速度矢量和旋转角速度矢量的个数，j＝1,2,…n表示机械臂的关节个数；

6×n的雅克比矩阵如下：

7.根据权利要求2所述的容错型冗余度机械臂运动规划方法，其特征在于：

所述的步骤D中，实际和期望的机械臂末端执行器轨迹之间的向量值偏误差计算公式为：

d(t)＝r(t)-r_d(t) (6)；

其中，d(t)为机械臂末端执行器轨迹之间的向量值偏误差，r(t)为关节空间的关节角向量，r_d(t)为期望的机械臂末端轨迹。

8.根据权利要求7所述的容错型冗余度机械臂运动规划方法，其特征在于：

所述的步骤D中，利用实际和期望的机械臂末端执行器轨迹之间的向量值偏误差优化后的笛卡尔速度方程的过程为：

a、对实际和期望的机械臂末端执行器轨迹之间的向量值偏误差求导，求导公式为

其中，为实际和期望的机械臂末端轨迹之间的向量值偏差随时间的导数，d(t)为机械臂末端执行器轨迹之间的向量值偏误差；其中参数λ(t)＝λ₀[1-cos(2πt/T)]，λ₀是一个可任意设置的标量系数，T表示机械臂执行任务的周期；

b、结合步骤C得到的笛卡尔速度方程和实际和期望的机械臂末端执行器轨迹之间的向量值偏误差求导公式，得到优化后的笛卡尔速度方程：

其中，表示机械臂末端执行器优化后的速度矢量，为实际和期望的机械臂末端轨迹之间的向量值偏差随时间的导数。

9.根据权利要求2所述的容错型冗余度机械臂运动规划方法，其特征在于：

所述的步骤E中，机械臂关节速度的计算公式为：

其中，为机械臂关节速度，J⁺＝J^T(JJ^T)^-1是雅克比矩阵J的伪逆，表示机械臂末端执行器优化后的速度矢量。