Drake Acrobot示例线性化A矩阵与标准形式差异原因问询

问题描述 #

• 在MIT《欠驱动机器人》讲义3.4.1节中，对机械臂方程在固定点处进行线性化推导，得到理论形式的线性化状态矩阵$A_{lin}$。
• 验证自行实现的Acrobot线性化效果时，基于示例3.5（Acrobot与Cart-pole的LQR控制）提供的Python notebook，在Acrobot LQR代码块第21行添加print(linearized_acrobot.A())语句，读取Examples模块中Acrobot Plant线性化后的A矩阵，发现该矩阵右下角2×2子块为非零值，与讲义推导的矩阵形式存在明显差异。

复现代码 #

import matplotlib.pyplot as plt
import mpld3
import numpy as np
from IPython.display import HTML, display
from pydrake.all import (AddMultibodyPlantSceneGraph, ControllabilityMatrix,
                         DiagramBuilder, Linearize, LinearQuadraticRegulator,
                         MeshcatVisualizerCpp, Parser, Saturation, SceneGraph,
                         Simulator, StartMeshcat, WrapToSystem)
from pydrake.examples.acrobot import (AcrobotGeometry, AcrobotInput,
                                      AcrobotPlant, AcrobotState)

from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve

from underactuated import FindResource, running_as_notebook
from underactuated.meshcat_cpp_utils import MeshcatSliders
from underactuated.quadrotor2d import Quadrotor2D, Quadrotor2DVisualizer

if running_as_notebook:
    mpld3.enable_notebook()

def UprightState():
    state = AcrobotState()
    state.set_theta1(np.pi)
    state.set_theta2(0.)
    state.set_theta1dot(0.)
    state.set_theta2dot(0.)
    return state

def acrobot_controllability():
    acrobot = AcrobotPlant()
    context = acrobot.CreateDefaultContext()

    input = AcrobotInput()
    input.set_tau(0.)
    acrobot.get_input_port(0).FixValue(context, input)

    context.get_mutable_continuous_state_vector()\
                .SetFromVector(UprightState().CopyToVector())

    linearized_acrobot = Linearize(acrobot, context)
    print(linearized_acrobot.A())
    print(
        f"The singular values of the controllability matrix are: {np.linalg.svd(ControllabilityMatrix(linearized_acrobot), compute_uv=False)}"
    )


acrobot_controllability()

差异原因 #

该差异来自两个模型的参数设定区别：

• 讲义中的线性化推导基于无关节阻尼的理想Acrobot模型，动力学方程中没有和关节角速度相关的耗散项，因此在竖直向上的平衡固定点做线性化时，角速度对时间导数的雅可比矩阵（即A矩阵右下角2×2子块）为零矩阵，和推导结果一致。
• Drake官方实现的AcrobotPlant默认给两个转动关节添加了粘性阻尼项，阻尼力矩大小与关节角速度成正比、方向与角速度方向相反，属于速度相关的耗散力。对带阻尼的模型在固定点线性化时，阻尼项会直接贡献到A矩阵的右下角2×2子块，因此会出现非零值。

如果需要得到和讲义完全匹配的线性化结果，将AcrobotPlant的两个关节阻尼参数设置为0后再执行线性化即可。

内容的提问来源于stack exchange，提问作者Noah Wechtler