CN111482968A - 一种基于bfs算法的六自由度偏置机器人逆解方法 - Google Patents

Abstract

本发明属于机器人逆运动学领域，具体涉及一种基于BFS算法的六自由度偏置机器人逆解方法。该方法首先将手腕无偏置型机器人运动学逆解的解析解作为求手腕偏置型机器人运动学逆解的迭代起点，其后通过齐次坐标变换矩阵构建适应度函数F(X)，最后通过BFS算法，不断迭代逼近，求得满足精度要求的手腕偏置型机器人运动学逆解。该方法因迭代出发点选择为手腕无偏置型机器人运动学逆解的解析解，更具有针对性，且选择BFS算法，有更快的收敛性，只需要求解一次雅可比矩阵，相较于传统的逆解算法，大大减小了计算量，提高实时性。

Description

一种基于BFS算法的六自由度偏置机器人逆解方法

技术领域

本发明属于机器人逆运动学领域，具体涉及一种六自由度偏置机器人的逆运动学通用求解方法。

背景技术

逆运动学算法是机器人运动学的重要组成部分，通过逆解算法求得各任务轨迹点处各关节角度值，据此可建立轨迹函数，进一步找到各关节位置、速度、加速度。为了得到封闭形式的位置逆解，机器人一般采用所谓的球形手腕，其3个转动关节轴线交于一点，恰好满足Pieper给出的具有封闭解的条件，然而，球形手腕结构也有着很大的局限性，并不总能满足要求。一方面是由于机械结构限制中间关节无法实现360°转动，从而限制了机器的灵活性；另一方面是由于三轴相交的限制使其结构强度受限，不能满足操作大负荷的要求。

为了解决这些问题，不得不使用所谓“偏置手腕”的结构形式。限于目前的数学工具，偏置型机器人的逆运动学问题一般没有实用的封闭解，通常采用数值解法。数值法主要有牛顿-拉夫森法、优化算法和迭代搜索类算法等，此外还有学者尝试使用人工神经网络方法、牛顿-拉夫森法及其改进算法。这些算法均存在算法复杂、运算量大的问题。

发明内容

本发明为了克服上述现有技术的不足，故在此提出一种基于BFS(Broyden-Fletcher-Shanno)算法的六自由度偏置机器人逆解方法。本发明的精度高、收敛性快，求解过程更加实时、快速、准确。

为实现上述目的，本发明公开的方法具体包括以下步骤。

S1，构建六自由度偏置机器人的sdh模型：六自由度偏置机器人由基座、末端执行器和5个连杆以及6个旋转关节组成；基于标准的DH参数法建立各关节坐标系，坐标系0为基坐标系，坐标系i为建立在连杆i的末端即旋转关节i+1上的坐标系，坐标系i具有X_i、Y_i、Z_i轴，关节转角θ_i表示X_i轴与X_i-1轴的夹角，i＝1,2,3,4,5,6，坐标系6为末端执行器坐标系。

S2，将六自由度偏置机器人的末端坐标系的目标位姿当作手腕无偏置型机器人的末端坐标系的位姿，求得手腕无偏置型机器人的运动学逆解的解析解即旋转关节的无偏置关节转角θ₁′至θ₆′，作为迭代初始点X₀。

S3，构建适应度函数F(X)：

。

其中X为旋转关节的关节转角θ₁至θ₆，

表示引入偏置参数的坐标系6相 对于坐标系0的齐次坐标变换矩阵的第p行第q列元素，p=1,2,3，q=1,2,3,4，nx、ny、nz分别 为坐标系6的X₆轴与坐标系0的X₀、Y₀、Z₀轴的夹角余弦值；ox、oy、oz分别为坐标系6的Y₆轴与 坐标系0的X₀、Y₀、Z₀轴的夹角余弦值；ax、ay、az分别为坐标系6的Z₆轴与坐标系0的X₀、Y₀、Z₀ 轴的夹角余弦值；px、py、pz为坐标系6的原点在坐标系0中的笛卡尔坐标。

S4，根据BFS算法求取六自由度偏置机器人的逆运动学解，具体包括以下步骤。

S41，取初始点X₀，置迭代次数k=0。

S42，对F(X)中各元素求偏导，求得雅可比矩阵J：

。

S43，代入旋转关节的关节转角X_k，计算k次迭代中的误差E，误差E取1-范数或者2-范数，并进行判断是否跳出循环，若E小于阈值或者迭代次数k达到最大迭代次数，则跳出循环，返回此时的X_k为六自由度偏置机器人的运动学逆解，否则进入下一次迭代。

S44，置迭代次数k=k+1，更新第k+1次迭代中X_k+1的值：

X_k+1=X_k-B_k*F(X_k)；其中，B_k=B_k-1+(s^k-1-B_k-1y^k-1)(s^k-1)^TB_k-1/(s^k-1)^TB_k-1y^k-1， s^k-1=X_k-X_k-1，

y^k-1=F(X_k)-F(X_k-1)。B₀ 直接通过公式B₀=inv(λI+J) 计算，其中I为6*6单位矩阵，inv为倒数函数，λ为修正因子，λ满足：

λ∈(-δ,ω)，δ=min(|ψ_i|²/2Reψ_i, Reψ_i>0)，ω= min(|ψ_i|²/-2Reψ_i, Reψ_i<0)，其中ψ_i为J的特征值，Re为取复数实部的数学运算符号，若不存在Reψ_i>0或Reψ_i<0的特征值可分取δ=+∞或ω=+∞。

S45，转步骤S43。

附图说明

图1为本发明涉及的一种六自由度偏置机器人的空间坐标系示意图。

图2为本发明涉及的算法流程图。

图3为本发明涉及的迭代结果图。

具体实施方式

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

本发明的机器人包括工业机器人、多关节机械手或多自由度的机器装置。如图1所示，六自由度偏置机器人由基座、末端执行器和5个连杆以及6个旋转关节组成。该六自由度偏置机器人的逆解的具体求法包括以下步骤。

S1，构建六自由度偏置机器人的sdh模型：基于标准的DH参数法建立各关节坐标系，坐标系的具体设置如下。

首先确定基坐标系，基座标系的选择几乎是任意的，可以选择将基坐标系的原点放置在基座关节轴线Z₀轴的任意位置，图1中放置于最下方，建立坐标系0。其次确定Z_i轴，其方向与关节轴线方向保持一致。然后确立X_i方向，当Z_i轴和Z_i+1轴不共面时，Z_i轴和Z_i+1轴的公垂线定义为X_i轴，X_i轴与Z_i轴的交点即为坐标系i的原点；当Z_i轴平行于Z_i+1轴时， Z_i轴和Z_i+1轴之间存在无穷多个共同法线，将穿过坐标系i+1的原点的法线选作X_i轴，坐标系i的原点是该法线与Z_i轴的交点；当Z_i轴和Z_i+1轴相交时，选择X_i轴垂直于Z_i轴和Z_i+1轴的交点，不过Z_i轴上的任意一点都可选做坐标系i的原点。最后Y_i轴的方向由Z_i轴和X_i轴确定，以Z_i轴为右手拇指，依据右手定则确定。根据上述规则从基座往机器人末端方向，依次建立7个坐标系，坐标系0为基坐标系，坐标系i为建立在连杆i的末端即旋转关节i+1上的坐标系，坐标系i具有X_i、Y_i 、Z_i轴，关节转角θ_i表示X_i轴与X_i-1轴的夹角，i为从基座到机器人末端的旋转关节编号，依次为1、2、3、4、5、6，坐标系6为末端执行器坐标系，如图1所示，L1到L7为各个关节坐标系原点之间的距离，菱形表示转动轴平行于纸面，圆环表示转动轴垂直于直面。

S2，将六自由度偏置机器人的末端坐标系的目标位姿当作手腕无偏置型机器人的末端坐标系的位姿，求得手腕无偏置型机器人的运动学逆解的解析解即旋转关节的无偏置关节转角θ₁′至θ₆′，作为迭代初始点X₀。

S21，建立相邻坐标系的各齐次变换矩阵：

其中连杆扭角α_i表示Z_i-1轴和Z_i轴的夹角，连杆长度a_i表示坐标系i和坐标系i-1的公垂线线段距离，连杆距离d_i表示坐标系i的原点在坐标系i-1的Z_i-1轴上的投影距离坐标系i-1的原点的距离；偏移角offset表示机器人的初始时刻θ_i的角度值。本专利具体选用的sdh参数如表1所示。

i	α<sub>i</sub>	a<sub>i</sub>	d<sub>i</sub>	θ<sub>i</sub>′	offset
						1	pi/2	0.050	0.3215	θ<sub>1</sub>′	0
2	0	0.270	0	θ<sub>2</sub>′	pi/2
						3	pi/2	0.07	0	θ<sub>3</sub>′	0
4	pi/2	0	0.299	θ<sub>4</sub>′	0
						5	- pi/2	0	0.1	θ<sub>5</sub>′	pi/2
6	0	0	0.0785	θ<sub>6</sub>′	0

表1 机器人的参数设置

计算1-3旋转关节的无偏置关节转角θ₁′至θ₃′，其满足

其中[X_d Y_d Z_d 1]^T表示无偏置手腕末端位置点的向量表达式，解方程组可以求出1-3旋转关节的无偏置关节转角θ₁′至θ₃′。

计算4-6旋转关节的无偏置关节转角θ₄′至θ₆′，其满足：

。

其中

为θ₄′=0时，坐标系6相对于坐标系4的旋转矩阵，

为 θ₄′=0时，坐标系4相对于基坐标系的旋转矩阵的逆矩阵，

为未引入偏置参数的坐标系6 相对于坐标系0的旋转矩阵，其可通过常规计算得出。通过从而解方程组可以求出4-6旋转 关节的无偏置关节转角θ₄′至θ₆′。最后三个关节通常有两种解，因此这种机器人的解的总数 就是前三个关节解的数量的2倍。

S3，构建适应度函数F(X)：

其中X为旋转关节的关节转角θ₁至θ₆，

表示引入偏置参数的坐标系6相对 于坐标系0的齐次坐标变换矩阵的第p行第q列元素，p=1,2,3，q=1,2,3,4，nx、ny、nz分别为 坐标系6的X₆轴与坐标系0的X₀、Y₀、Z₀轴的夹角余弦值；ox、oy、oz分别为坐标系6的Y₆轴与坐 标系0的X₀、Y₀、Z₀轴的夹角余弦值；ax、ay、az分别为坐标系6的Z₆轴与坐标系0的X₀、Y₀、Z₀轴 的夹角余弦值；px、py、pz为坐标系6的原点在坐标系0中的笛卡尔坐标。

其中引入偏置参数的坐标系6相对于坐标系0的齐次坐标变换矩阵可根据矩阵连乘求出：

。

其中，

。

S4，根据BFS算法求取六自由度偏置机器人的逆运动学解，具体包括以下步骤。

S41，取初始点X₀，置迭代次数k=0。

S42，对F(X)中各元素求偏导，求得雅可比矩阵J：

。

S43，代入旋转关节的关节转角X_k，计算k次迭代中的误差E，误差E取1-范数或者2-范数，并进行判断是否跳出循环，若E小于阈值或者迭代次数k达到最大迭代次数，则跳出循环，返回此时的X_k为六自由度偏置机器人的运动学逆解，否则进入下一次迭代。

S44，置迭代次数k=k+1，更新第k+1次迭代中X_k+1的值：

X_k+1=X_k-B_k*F(X_k)；

其中，B_k=B_k-1+(s^k-1-B_k-1y^k-1)(s^k-1)^TB_k-1/(s^k-1)^TB_k-1y^k-1， s^k-1=X_k-X_k-1，y^k-1=F(X_k)-F(X_k-1)。B₀ 直接通过公式B₀=inv(λI+J) 计算，其中I为6*6单位矩阵，inv为倒数函数，λ为修正因子，λ满足：

λ∈(-δ,ω)，δ=min(|ψ_i|²/2Reψ_i, Reψ_i>0)，ω= min(|ψ_i|²/-2Reψ_i, Reψ_i<0)，其中ψ_i为J的特征值，Re为取复数实部的数学运算符号，若不存在Reψ_i>0或Reψ_i<0的特征值可分取δ=+∞或ω=+∞。

S45，转步骤S43。

实验采用matlab中机器人工具箱进行仿真，取最大迭代次数为100。迭代结果如图3所示，横轴表示迭代次数，纵轴表示误差E的值，显然在较少的迭代次数内可以快速收敛， E小于1*e^-10，可认为一致。

综上所述，本发明利用同构型无偏置六自由度机器人的逆解作为迭代的起点，很大程度上保证了迭代初值在可局部收敛范围内，同时引入修正因子求雅可比矩阵的逆，消除了雅可比矩阵的奇异性。通过迭代可得逆运动学唯一解，此解等同于按能量最小原则确定的最终解。此算法精度高、收敛性快，只需要求解一次雅可比矩阵，能够实时快速、准确地计算出六自由度偏置机器人的逆解。

Claims (3)

1.一种基于BFS算法的六自由度偏置机器人逆解方法，其特征在于，包括以下步骤：

S1，构建六自由度偏置机器人的sdh模型：六自由度偏置机器人由基座、末端执行器和5个连杆以及6个旋转关节组成；基于标准的DH参数法建立各关节坐标系，坐标系0为基坐标系，坐标系i为建立在连杆i的末端即旋转关节i+1上的坐标系，坐标系i具有X_i、Y_i、Z_i轴，关节转角θ_i表示X_i轴与X_i-1轴的夹角，i＝1,2,3,4,5,6，坐标系6为末端执行器坐标系；

S2，将六自由度偏置机器人的末端坐标系的目标位姿当作手腕无偏置型机器人的末端坐标系的位姿，求得手腕无偏置型机器人的运动学逆解的解析解即旋转关节的无偏置关节转角θ₁′至θ₆′，作为迭代初始点X₀；

S3，构建适应度函数F(X)：

；

其中X为旋转关节的关节转角θ₁至θ₆，

表示引入偏置参数的坐标系6相对于 坐标系0的齐次坐标变换矩阵的第p行第q列元素，p=1,2,3，q=1,2,3,4，nx、ny、nz分别为坐 标系6的X₆轴与坐标系0的X₀、Y₀、Z₀轴的夹角余弦值；ox、oy、oz分别为坐标系6的Y₆轴与坐标 系0的X₀、Y₀、Z₀轴的夹角余弦值；ax、ay、az分别为坐标系6的Z₆轴与坐标系0的X₀、Y₀、Z₀轴的 夹角余弦值；px、py、pz为坐标系6的原点在坐标系0中的笛卡尔坐标；

S4，根据BFS算法求取六自由度偏置机器人的逆运动学解，具体包括以下步骤：

S41，取初始点X0，置迭代次数k=0；

S42，对F(X)中各元素求偏导，求得雅可比矩阵J：

；

S43，代入旋转关节的关节转角X_k，计算k次迭代中的误差E，误差E取1-范数或者2-范数，并进行判断是否跳出循环，若E小于阈值或者迭代次数k达到最大迭代次数，则跳出循环，返回此时的X_k为六自由度偏置机器人的运动学逆解，否则进入下一次迭代；

S44，置迭代次数k=k+1，更新第k+1次迭代中X_k+1的值：

X_k+1=X_k-B_k*F(X_k)；

其中，B_k=B_k-1+(s^k-1-B_k-1y^k-1)(s^k-1)^TB_k-1/(s^k-1)^TB_k-1y^k-1， s^k-1=X_k-X_k-1，y^k-1=F(X_k)-F(X_k-1)，B₀ 直接通过公式B₀=inv(λI+J) 计算，其中I为6*6单位矩阵，inv为倒数函数，λ为修正因子，λ满足：λ∈(-δ,ω)，δ=min(|ψ_i|²/2Reψ_i, Reψ_i>0)，ω= min(|ψ_i|²/-2Reψ_i, Reψ_i<0)，其中ψ_i为J的特征值，Re为取复数实部的数学运算符号，若不存在Reψ_i>0或Reψ_i<0的特征值可分取δ=+∞或ω=+∞；

S45，转步骤S43。

2.如权利要求1所述的一种基于BFS算法的六自由度偏置机器人逆解方法，其特征在于，步骤S2的具体操作步骤包括：

S21，建立相邻坐标系的各齐次变换矩阵：

其中连杆扭角α_i表示Z_i-1轴和Z_i轴的夹角，连杆长度a_i表示坐标系i和坐标系i-1的公垂线线段距离，连杆距离d_i表示坐标系i的原点在坐标系i-1的Z_i-1轴上的投影距离坐标系i-1的原点的距离；

S22，计算1-3旋转关节的无偏置关节转角θ₁′至θ₃′，其满足：

其中[X_d Y_d Z_d 1]^T表示无偏置手腕末端位置点的向量表达式，解方程组求出1-3旋转关节的无偏置关节转角θ₁′至θ₃′；

S23，计算4-6旋转关节的无偏置关节转角θ₄′至θ₆′，其满足：

；

其中

为θ₄′=0时，坐标系6相对于坐标系4的旋转矩阵，

为θ₄′=0时，坐标系4相对于基坐标系的旋转矩阵的逆矩阵，

为未引 入偏置参数的坐标系6相对于坐标系0的旋转矩阵，通过解方程组可以求出4-6旋转关节的 无偏置关节转角θ₄′至θ₆′。

3.如权利要求1或2所述的一种基于BFS算法的六自由度偏置机器人逆解方法，其特征在于，最大迭代次数为100，阈值为1*e^-10。

Images