库卡机器人速度控制器

Question

我正在尝试使用控制 Lyapunov 函数使我的 Kuka 末端执行器达到预期目标。我正在关注这篇论文：http://skrifo.juxi.net/papers/zhuang2019iros.pdf

Lyapunov 函数是 here, and this 是其关于关节角度的偏导数。

到目前为止，控制器似乎在做一些古怪的事情——Lyapunov 函数似乎并没有在所有时间步骤中减少 [assert(V <= prev_V + 0.001) 失败！]。 Lyapunov 函数似乎是正确的，但我认为问题出在偏导数的计算上。雅可比计算在我的脑海中是有意义的（我希望输出在末端执行器框架中），但显然有些地方不对：

jacobian = robot.CalcJacobianSpatialVelocity(
    context=differential_ik.GetRobotContext(), with_respect_to=JacobianWrtVariable.kQDot,
    frame_B=robot.world_frame(),                    # frame on which point Bi is fixed
    p_BP=[0.0, 0.0, 0.0],                           # position vec from frame_B's origin to points Bi
    frame_A=robot.GetFrameByName("iiwa_link_7"),    # frame that measures v_ABi
    frame_E=robot.GetFrameByName("iiwa_link_7"))    # frame that v_ABi is expressed on input 
                                                    # and frame that Jacobian J_V_ABi is expressed on output

[我将 GetRobotContext() 添加到 DifferentialIK class 但本可以使用 GetSubsystemContext()]

我正在使用 q = q_prev + time_step * q_dot 设置 Drake 中的速度，然后在 q 上设置 differential_ik.SetPositions()。

这是我可以创建的最小工作代码来复制我的问题（抱歉，它仍然有点大）：https://gist.github.com/maggi3wang/1cbb02a0adc2cec85915f856ab3d1702#file-small_collect_data-py-L174

如有任何帮助，我们将不胜感激！

Answer 1

我想说调试程序的最快方法是检查 ∂V / ∂θ 是否计算正确（我认为这是代码中的变量 pdV）。你可以通过数值差来计算这个梯度，然后将数值梯度与你的梯度进行比较。

我试图推导出下面的控制律：

假设您希望您的 ᵂX⁷ ∈ SE(3)（link 7 的姿态在世界坐标系中测量和表达）与一些所需的姿态 ᵂX⁷_des 相匹配，并且您将 Lyapunov 函数定义为

V = |ᵂX⁷ - ᵂX⁷_des|_F

这里我用 | |_F 来表示 Frobenius 范数。我将使用 ᵂR⁷ 表示旋转矩阵，并使用 ᵂp⁷ 表示 link 7 在世界坐标系中的位置。

这个李雅普诺夫函数等价于

V = |ᵂR⁷ - ᵂR⁷_des|_F + (ᵂp⁷ - ᵂp⁷_des)ᵀ(ᵂp⁷ - ᵂp⁷_des)
  = trace((ᵂR⁷ - ᵂR⁷_des)ᵀ(ᵂR⁷ - ᵂR⁷_des)) + (ᵂp⁷ - ᵂp⁷_des)ᵀ(ᵂp⁷ - ᵂp⁷_des)
  = 6 - 2*trace(ᵂR⁷_desᵀ * ᵂR⁷) + (ᵂp⁷ - ᵂp⁷_des)ᵀ(ᵂp⁷ - ᵂp⁷_des)

您的目标是计算梯度 ∂V / ∂θ。要计算此偏导数，您需要偏导数 ∂ᵂR⁷ / ∂θ 和 ∂ᵂp⁷ / ∂θ。当你从 CalcJacobianSpatialVelocity 计算雅可比行列式时，这个雅可比行列式的平移部分是 ∂⁷pᵂ / ∂θ （顺便说一句，我更习惯用世界框架来表达 position/velocity 而不是 link 7 的框架), 但这个雅可比行列式的旋转部分是 而不是 ∂ᵂR⁷ / ∂θ。假设你打电话给

jacobian = robot.CalcJacobianSpatialVelocity(
    context=differential_ik.GetRobotContext(), with_respect_to=JacobianWrtVariable.kQDot,
    frame_B=robot.GetFrameByName("iiwa_link_7"),
    p_BP=[0.0, 0.0, 0.0],
    frame_A=robot.world_frame(),
    frame_E=robot.world_frame())

，这个雅可比行列式的旋转部分是∂ ᵂω⁷/∂ θ_dot，其中ᵂω⁷是link 7在世界坐标系中测量和表达的angular速度。为了计算∂ᵂR⁷ / ∂θ，我们使用下面的等式

∂ᵂR⁷ / ∂θ = ∂ᵂR_dot⁷ / ∂θ_dot
          = ∂(⌊ᵂω⁷⌋ₓ ᵂR⁷) / ∂θ_dot
          = (∂⌊ᵂω⁷⌋ₓ / ∂θ_dot ) * ᵂR⁷

在等式的第一行，我对分子和分母都求了时间导数。在等式的第二行，我使用等式 ᵂR_dot⁷ = ⌊ᵂω⁷⌋ₓ ᵂR⁷，其中 ⌊ᵂω⁷⌋ₓ 表示 angular 速度 ᵂω⁷ 的斜对称矩阵（顺便说一句，我忘了是不是是 ᵂR_dot⁷ = ⌊ᵂω⁷⌋ₓ ᵂR⁷ 或 ᵂR_dot⁷ = ᵂR⁷⌊ᵂω⁷⌋ₓ。如果一个不起作用，你可以尝试另一个）等式的最后一行通过乘积的梯度和 ∂ᵂR⁷ / ∂θ_dot = 0 成立。

库卡机器人速度控制器

Velocity Controller for Kuka Robot

controller

drake