机械臂笔记（四）通过雅可比矩阵实现逆运动学运算
通过雅可比矩阵可以较快的完成逆运动学运算，但是全程是通过计算当前位置与目标位置的差距（向量），来进行一步步迭代修正。可以理解为走一步看一步。
假如有向量函数
已知当前状态：
既然有目标状态和函数式
写成向量的方式如下：
其中矩阵
反过来根据
根据实际情况
- 计算机械臂末端距离目标的向量方向和大小。如果小于一定阈值则停止计算；
- 根据当前关节角度计算雅可比矩阵；
- 计算雅可比矩阵的逆矩阵；
- 根据逆矩阵计算每个关节需要旋转的角度；
- 完成旋转操作；
- 返回步骤1，直到精度满足需要或达到循环次数限制。
程序设计（仅做位置IK）
| 当前坐标系 | 下一个关节 | ||||
|---|---|---|---|---|---|
参照之前的参数表，首先我们需要Sympy 能够将推导的公式固化下来。
python
import jax
import jax.numpy as jnp
from typing import List, Sequence,Tuple
jax.config.update("jax_enable_x64", True) # jax 默认不支持64位浮点数运算，需要手动启用
# 某一个MDH 关节的MDH 参数矩阵构建
def mdh(alpha: float, a: float, theta: float, d: float) -> jnp.ndarray:
cos_alpha = jnp.cos(alpha)
sin_alpha = jnp.sin(alpha)
cos_theta = jnp.cos(theta)
sin_theta = jnp.sin(theta)
return jnp.array([
[cos_theta, -sin_theta, 0, a],
[sin_theta*cos_alpha, cos_alpha*cos_theta, -sin_alpha, -d*sin_alpha],
[sin_alpha*sin_theta, sin_alpha*cos_theta, cos_alpha, d * cos_alpha],
[0, 0, 0, 1]
],dtype=jnp.float64)
# 根据MDH 表都构建参数矩阵
def mdhs(params: List[Tuple[float, float, float, float]])->jnp.ndarray:
t = jnp.eye(4)
for param in params:
(alpha, a, theta, d) = param
t = t @ mdh(alpha, a, theta, d)
return t
# 指定机型（固化了某些参数）
def mdh_puma_560(theta:List[float]):
alpha = [0, -jnp.pi/2, 0, -jnp.pi/2, jnp.pi/2, -jnp.pi/2]
a = [0, 0, 0.431, 0.020, 0, 0]
d = [0, 0, 0.149, 0.433, 0, 0]
return mdhs(zip(alpha,a,theta,d))[:3,3]
### 之所以取[:3, 3] 是只计算最终位置，加上姿态难度有些太大了
jac_puma_560 = jax.jacobian(mdh_puma_560) # 推导puma 560 的雅可比矩阵
target_pos = jnp.array([ # 目标位置
[-0.344, 0.923, -0.170, 0.213],
[0.398, 0.307, 0.864, 0.847],
[0.850, 0.230, -0.474, -0.078],
[0.000, 0.000, 0.000, 1.000]
])[:3,3]
theta0 = jnp.zeros(6)
init_pos = mdh_puma_560(theta0) # 初始位置
step = 0.5 # 每次角度变换的步长系数
for i in range(0, 1000):
d_pose = target_pos-init_pos # 计算位置查（向量）
jac_pose = jac_puma_560(theta0) # 计算雅可比矩阵
jac_pose_pinv = jnp.linalg.pinv(jac_pose) # 计算雅可比矩阵的逆矩阵
d_theta = jac_pose_pinv@d_pose # 计算角度变化
theta0 = theta0 + d_theta*step # 更新角度
init_pos = mdh_puma_560(theta0) # 更新末端位置
if jnp.linalg.norm(d_pose) < 1e-4: # 当误差值小于允许之后就退出循环
deg = jnp.rad2deg(theta0)
print(deg)
break
# 没有结果，失败一般循环20次左右就能得到结果。比前面神经网络要快得多。
| 方法 | 更新公式 |
|---|---|
| 梯度下降 | Δθ = −α Jᵀ e |
| 伪逆 IK | Δθ = J⁺ e |
| DLS | Δθ = Jᵀ (JJᵀ + λI)⁻¹ e |
程序设计（包含姿态IK）
如果要包含姿态的话，就需要在计算误差时将姿态误差也体现出来。最简单的就是把DMH 的输出展平，事实上只要能将MDH 最终的结果辗成(nx1) 维即可：
python
def mdh_puma_560(theta:List[float]):
alpha = [0, -jnp.pi/2, 0, -jnp.pi/2, jnp.pi/2, -jnp.pi/2]
a = [0, 0, 0.431, 0.020, 0, 0]
d = [0, 0, 0.149, 0.433, 0, 0]
return mdhs(zip(alpha,a,theta,d)).flatten()
target_pos = jnp.array([
[-0.344, 0.923, -0.170, 0.213],
[0.398, 0.307, 0.864, 0.847],
[0.850, 0.230, -0.474, -0.078],
[0.000, 0.000, 0.000, 1.000]
]).flatten()
# 这种试验结果是最好的后面的代码不用做任何修改。基本10~20 次迭代也能达到不错的效果。如果剔除原矩阵的第四行也不错。
python
def mdh_puma_560(theta:List[float]):
alpha = [0, -jnp.pi/2, 0, -jnp.pi/2, jnp.pi/2, -jnp.pi/2]
a = [0, 0, 0.431, 0.020, 0, 0]
d = [0, 0, 0.149, 0.433, 0, 0]
return mdhs(zip(alpha,a,theta,d))[:4, :3].flatten()
target_pos = jnp.array([
[-0.344, 0.923, -0.170, 0.213],
[0.398, 0.307, 0.864, 0.847],
[0.850, 0.230, -0.474, -0.078],
[0.000, 0.000, 0.000, 1.000]
])[:4, :3].flatten()因为机械臂末端一般只有一个旋转的自由度（夹抓或焊枪），所以也可以只取末端的z 轴（或z+x 轴）和位置作为判断目标：
python
def mdh_puma_560(theta:List[float]):
alpha = [0, -jnp.pi/2, 0, -jnp.pi/2, jnp.pi/2, -jnp.pi/2]
a = [0, 0, 0.431, 0.020, 0, 0]
d = [0, 0, 0.149, 0.433, 0, 0]
T = mdhs(zip(alpha,a,theta,d))
return jnp.concatenate([T[:3,3], T[:3,2]])上面代码测试结果有时候会出现角度非常大的情况，基本上将其缩放至0~360 之内就还可以。
自适应步长
自适应步长可以在非常接近目标值时依然能保持较好的收敛，但是结果就是收敛速度比较慢。
python
prev_d_pose = target_pos-init_pos
for i in range(0, 1000):
# ...
if jnp.linalg.norm(prev_d_pose) < jnp.linalg.norm(d_pose):
step *= 1.05
else:
step *=0.95
prev_d_pose = d_pose
# ...轴角
通过SE3 据说也能算，但是我在测试是会遇到不收敛的的问题。以后再说吧
避免角度过大的问题
- 增加阻尼
- 添加物理限位
python
joint_min = jnp.array([
-160, -225, -225, -110, -100, -266
]) * jnp.pi / 180.0
joint_max = jnp.array([
160, 45, 45, 170, 100, 266
]) * jnp.pi / 180.0
for i in range(0, 1000):
# ...
theta0 = theta0 + d_theta*step # 更新角度
theta0 = jnp.clip(theta0, joint_min, joint_max)