雅可比矩阵在机器人运动中的应用

纳文, 张世强, 曹越琦, 孙华飞

(北京理工大学 数学与统计学院, 北京 100081)

随着机器人的应用范围不断扩大, 对机器人的运动学、控制以及轨迹规划等要求不断提高. 上述各个方面的研究进展制约着机器人的表现. 本文利用雅可比矩阵来研究带有冗余度的机器人的运动、特别是逆运动学问题. 众所周知, 机器人的正运动学相对简单, 逆运动学比较复杂, 因为对于给定的机器人的末端执行器的位置和方向, 要求得连杆的关节角, 这个问题有时无解或者存在多解现象. 另一方面, 尽管冗余度的存在增加了解决问题的复杂性, 但是可以利用冗余度来解决诸如避障、躲避奇异点等问题[1-6]. 这些研究与雅可比矩阵密切相关.

在本文中, 利用雅可比矩阵首先介绍机器人的逆运动学, 然后给出带有冗余度的逆运动学的应用, 特别地给出模拟仿真, 为后续的机器人的运动规划和控制的实现奠定基础.

1 逆运动学

1.1 带有冗余度的逆运动学

设机器人的位姿为x, 它包括位置和方向角, 关节角为θ. 于是它们满足

x=f(θ).

(1)

已知关节角来求位姿属于正运动学问题, 这个过程相对简单, 而实际问题往往要求给定位姿来求解关节角,这个过程称为逆运动学, 这个过程的求解比较困难. 目前通常的方法是利用式(1) 获得.

(2)

(3)

(4)

式中J*(θ)=g-1JT(θ)[J(θ)g-1JT(θ)]-1,为J(θ)的广义逆.

下面考虑带有具有冗余度的情形(m

(5)

式中：I为n×n单位矩阵；ξ为任意的向量.

冗余度的存在使得问题变得复杂, 而利用冗余度的存在, 也可以说在解决避障等问题的同时而获得关节角速度.

(6)

式中θ0是期望达到的或者期望避开的. 当想要避开θ0时,ψ(θ)要取最大值,于是要求k>0. 反之,当要达到θ0时,ψ(θ)要取最小值, 要求k<0.

对于rank(J(θ))

(7)

其中μ为实常数. 可以得到

(8)

1.2 操作度

命题1.2.1带有最大操作度的角加速度可由下面公式给出[7-11]

(9)

(10)

式中：

A=J(θ)JT(θ)=(aij)m×m;

下面, 要给出J(θ)的值域. 注意到

J(θ)J*(θ)=I,

(J*(θ))T[I-J*(θ)J(θ)]T=

[(I-J*(θ)J(θ))J*(θ)]T=0.

同理可以得到

[I-J*(θ)J(θ)]TJ*(θ)=0.

由式(5)可以得到

{[I-J*(θ)J(θ)]ξ}T{[I-J*(θ)J(θ)]ξ}≥

(11)

由奇异值分解, 则存在正交矩阵U,V满足

JT(θ)=Udiag[σ1…σm0…0]VT,

其中,σi≥0. 于是可得

(12)

(13)

这是一个椭球方程.

1.3 梯度下降法

设机械臂执行器的理想位置为xd, 的目标就是让当前机械臂的执行器的位置f(θ)与xd的距离最短,于是问题可以转化为

用梯度下降法求解上述F(θ)的最小值. 既然

-JT(θ)(xd-f(θ)),

令0<η<1为常数, 有迭代公式

θt +1=θt-ηJT(θt)(xd-f(θt)),

由此迭代公式可获得F(θ)的局部最小值.

2 模拟仿真

考虑二维和三维的情况, 以向量L为机械臂每段的臂长, 向量θ为关节角,θ0为初始状态机械臂的关节角, 二维情况机械臂初始状态如图1所示(二维情况下的关节角指的是相邻两个机械臂的夹角).

其中机械臂每段臂长

L=[0.670.4320.4320.15],

初始状态机械臂的关节角为

θ0=[-0.595 12.720 90.492 1].

初始关节角对应的可操作度为0.082 1. 由式(12)(13)可知末端执行器的速度向量的变化范围是一个椭球.

三维情况下机械臂初始情况如图2所示，三维情况下的关节角指的是每个机械臂在球坐标下的两个角度.

其中机械臂每段臂长

L=[0.200.760.610.53],

初始状态机械臂的关节角为

θ0=[(0.50,0.23), (1.00,0.31), (1.50,0.50)].

初始关节角对应的可操作度为0.356 3.

2.1 rank(J(θ))=m且无冗余项

由迭代公式, 设定步长为0.005. 3个关节角时, 让机械臂用1 s从初始状态沿直线到达点(0.2,0.8), 三维情况下, 让机械臂用1 s从初始状态沿直线到达点(0.2,0.3,0.4), 其轨迹如图3所示.

2.2 rank(J(θ))=m且有冗余项

由迭代公式,其中为可操作度, 取c=1,设定步长为0.005. 3个关节角时, 让机械臂用1 s从初始状态沿直线到达点(0.4,0.5), 三维情况下, 让机械臂用1 s从初始状态沿直线到达点(-0.3,0.2,-0.2), 其轨迹如图4所示.

2.3 rank(J(θ))

由迭代公式,取μ2=0.001, 设定步长为0.005. 3个关节角时, 让机械臂用1 s从初始状态沿直线到达点(0.8,0.6), 三维情况下, 让机械臂用1 s从初始状态沿直线到达点(0.4,0.5,0), 其轨迹如图5所示.

2.4 梯度下降法求解逆运动学问题

由迭代公式θt+1=θt-ηJT(θt)(xd-f(θt)), 取步长η=0.5. 3个关节角时, 让机械臂不断沿梯度下降方向到达点(0.8,0.4), 三维情况下, 让机械臂不断沿梯度下降方向到达点(0.6,0.8,1.0), 其轨迹如图6所示.

3 结 论

通过分析雅克比矩阵的性质研究了机器人的逆运动问题. 在无冗余度时，利用伪逆可以直接求解机器人逆运动，给出终点位姿即可得到机械臂的关节角. 虽然冗余度的存在使得问题变得更加复杂，但利用冗余度通过阻尼最小平方法在解决避障等问题的同时获得了关节角速度. 在雅克比矩阵奇异时通过梯度下降法给出求解机器人逆运动的迭代公式. 在模拟仿真阶段，文中首先给出末端执行器速度向量的变化范围，实验结果与理论分析相符. 之后通过不同维数下设定直线轨迹求解关节角，证明了雅克比矩阵非奇异条件下阻尼最小平方法的正确性与高效性. 而在雅克比矩阵奇异的情况下，设定终点位姿，通过梯度下降法高效地给出了合法轨迹. 在不同维数和自由度的情形下均得到了很好的仿真结果，反映出理论方法的准确性和高效性.