具身智能3全身动力学控制软件包（人形机器人）摘自Openloong社区

系统介绍

易于部署，

易于理解，人形控制框架结构采用分层模块化设计，提高了系统的可维护性和可扩展性。

易于分层结构，遵循功能模块封装的代码设计原则，采用总线进行模块之间的数据交互，减少封装冗余，有利于降低代码复杂度；算法实现采用“读-计算-写”的简单逻辑，提高了代码的可读性。

功能模块

模型预测+全身动力学控制

分层模块化的软件框架

软件框架结构采用分层模块化设计，系统各功能模块在逻辑和功能上具有明确的界限，提高系统的可维护性和可扩展性，为二次开发提供了更加友好的环境

基于MPC+WBC的运动控制 （偏传统的，现在都RL）

高低层级结合的运动控制，高层级采用基于单刚体模型的MPC，低层级采用基于零空间映射的多任务优先级控制的WBC，提高机器人控制的稳定性，实现冗余自由度机器人的运动控制

整个控制框架图，如下所示，算法分为：步态控制、外部接触控制、内部多关节协同控制、底层关节跟踪与感知模块。

主要介绍下外部接触模块与内部多关节协同模块：
外部接触模块是基于单刚体模型，产生支撑腿的末端接触力及腾空腿的末端位置轨迹。其中：

（1）支撑腿采用模型预测控制即MPC，依靠QP，优化产生足底力，输入的系统状态为机身的姿态、位置、角速度与速度，优化目标有两个，一个是当前状态与期望的状态误差尽量小，一个是系统输入尽量小，即足底力与力矩尽量小，令系统能量输出较低，在该优化目标下设置了4组约束，

        1组是z方向半封闭约束，即z方向力不能小于0的值，
        2是摩擦力约束，利用摩擦锥约束，
        3是防止足底反转与扭转的力矩约束，即防止足底力矩过大导致脚面反转或者扭转，采用的是QPOASES进行QP解算，在此优化目标与约束下，产生末端接触力。
（2）对于腾空腿，是应用的基于速度进行x，y方向落脚点计算的方法，具体的计算步骤在四足机器人控制中有详解，就不赘述了，z方向是由贝塞尔曲线组合成的相位可调的轨迹，曲线如下图所示，在相位末端z会延长一段，这是为了保证腾空腿触地。
内部多关节协同控制，此部分主要是基于WBC，不同于前述的MPC，WBC是基于全身动力学模型的方法，将外部接触控制生成的末端力矩速度位置参考映射成为关节位置、速度、力矩参考。
他分为两个步骤：
        第一，是运动学求解器（逆运动学），根据机身当前和期望状态，生成关节的位置、速度、加速度，这部分是应用基于零空间投影的多任务优先级控制方法，低优先级任务会在高优先级解的范围内寻找解，不会干扰到高优先级任务，实现冗余自由度机器人的控制，任务一般会分成以下几个，足底接触、机身位置、机身速度、摆动腿轨迹、手部关节控制、冗余关节，这些任务可以根据实际进行顺序的调节，经过几个任务的累加，生成关节的位置、速度、加速度；
        第二，是动力学求解器，根据运动学求解器的输出还有期望的足底接触力，生成最终的关节参考，这里实际上是一个QP优化问题，对输入的期望足底接触力与关节的加速度的调节增量进行优化，设置动力学约束，来保证求解结果符合动力学模型，加入摩擦约束及接触力大小约束来保证求解结果在正常范围内，最终得到关节的位置、速度、力矩参考。

基于Pinocchio动力学库搭建的全身动力学框架

项目采用Pinocchio动力学库搭建了全部的模型预测控制与全身动力学模型，可以实现基于URDF模型的快速动力学参数调整

pinocchio仓库基于C++的开源动力学求解库，同时也提供了python的接口，可以根据输入的URDF模型进行机器人运动学及动力学的参数计算，相关安装步骤可参考官网，也可使用openloong-dyn-control中的third party
具体功能如下：
        动力学方程参数的求解；
        逆运动学解算；
        正运动学求解；
        雅可比矩阵求解。
Pinocchio在以上模块中的底层关节跟踪与感知模块中，其计算结果在控制算法中主要作用：
        一个是正逆运动学的求解，其获取的末端位置在摆动腿轨迹计算、WBC的任务优先级计算等模块都有应用，
        第二是任务优先级中的雅可比矩阵的获取，
        第三是WBC的QP求解中需要动力学约束，这里应用了pinocchio库计算的动力学方程参数，
        第四是状态估计中的足底力估计，也是应用到了动力学方程参数。
关于pinocchio应用的注意点如下所示：
1、由于人形机器人是个浮动基，q包含了质心位置、质心姿态、关节位置信息，其中前7个为浮动基的数据，浮动基的姿态采用四元数，因此q比dq多一维；
2、pinocchio中默认的关节顺序，是根据urdf中joint的名字及串联关系确定的，先根据串联关系，然后根据其首字母进行排列；
3、输入pinocchio的dq浮动基速度为本体坐标系下的速度；
4、urdf关节类型要选revolute而不是continuous，不然会q的个数会多一个；
5、获取雅可比的三种坐标形式，pinocchio中的雅可比有三种坐标形式，分别是WORLD、LOCAL、LOCAL_WORLD_ALIGNED，在获取雅可比时选取何种坐标决定了雅可比根据dq计算出的末端速度是在哪个坐标系下，
world坐标系是坐标轴朝向与世界坐标系平行，原点位于机身最初的base处，
local的坐标与雅可比求解的末端坐标重合，例如我们获取的是脚踝的雅可比，则local坐标系即与脚踝的坐标重合，
LOCAL_WORLD_ALIGNED的坐标系是指坐标轴朝向与世界一致，坐标轴原点与脚踝坐标系原点重合，openloong程序中雅可比应用的是LOCAL_WORLD_ALIGNED，因为程序中用的都是末端在世界坐标下的速度，而这个坐标系的坐标轴朝向与世界相同，因此速度符合算法中的要求。
6、逆运动学的求解，pinocchio官网中提供了一种闭环逆运动学的求解方法，迭代获得解，要注意为了避免奇异点问题，采用阻尼伪逆，这种方式能够减少求解时的震荡，官网中有逆运动学的范例程序，可以先用其中的默认值，如果出现求解问题，可以调节DT与参数，前面说DT类似于Kp，那就有类似于阻尼的作用，即Kd的作用，大家可以根据pd的作用机理去调节这两个参数。

Mujoco仿真环境

URDF文件

项目提供了青龙人形机器人包含上肢与下肢的全身URDF，同时面向mujoco仿真环境提供了XML模型文件

多样化的仿真场景

项目提供了多样化的仿真测试场景，双臂作业场景与模型

标准化的接口

项目面向运动捕捉操控构建了与实物样机一致的开发接口，可以实现在仿真环境下的示教与数据收集

可视化与调试工具

plot画图工具

可以完成对运动控制数据的绘制，提高调试开发效率

数据记录

提供了对仿真数据记录的工具，方便对数据进行后处理与分析

数据集

提供了面向模仿学习双臂作业数据存储的接口，方便在仿真中开发具身智能算法
 

开发基础知识

以下内容进行仔细分解

PVT 控制：PVT 控制的核心是 **“轨迹预先生成”**：
在运动开始前，根据目标位置、运动约束（如最大速度、加速度），计算出一条连续的轨迹，该轨迹上每个时刻的位置、速度均被明确定义。运动过程中，控制器只需按预设的时间点输出对应的位置指令，即可驱动执行机构（如电机）沿规划轨迹运动。

• 位置（P）：某一时刻执行机构应到达的空间坐标（如笛卡尔坐标系中的 X/Y/Z 值，或旋转轴的角度）。
• 速度（V）：对应时刻的运动速率（需与位置轨迹的斜率匹配，确保速度连续）。
• 时间（T）：每个位置和速度对应的时间节点（形成时间序列，如 t₀、t₁、t₂...）。

关键控制参数

模型预测控制参数 (MPC parameters)

//MPC.h
void set_weight(double u_weight, Eigen::MatrixXd L_diag, Eigen::MatrixXd K_diag);
//_u_weight_ : the minimal weight of control input
//_L_diag_ : the weight of error compared to desired values, following the order (eul, pos, omega, vel)
//_K_diag_ : the weight of control input, following the order (fl, tl, fr, tr)

全身控制优先级 (WBC priority)

//WBC_QP.cpp
std::vector<std::string taskOrder;
taskOrder.emplace_back("RedundantJoints");
taskOrder.emplace_back("static_Contact");
taskOrder.emplace_back("Roll_Pitch_Yaw_Pz");
taskOrder.emplace_back("PxPy");
taskOrder.emplace_back("SwingLeg");
taskOrder.emplace_back("HandTrack");

全身控制重量 (WBC weight)

//PriorityTasks.h
Eigen::MatrixXd Kp; //weight of position error
Eigen::MatrixXd Kd; //weight of velocity eror

//WBC_QP.h
Eigen::MatrixXd Q1; //weight of the contact force error compared to desired, following the order (fl, tl, fr, tr)
Eigen::MatrixXd Q2; //weight of the acceleration tracking error

摆腿轨迹 (Swing leg trajectory)

//FootPlacement.h
double kp_vx; //x-direction footplacement parameter
double kp_vy; //y-direction footplacement parameter
double kp_wz; //z-direction posture parameter
double stepHeight; //the maximal step height

//FootPlacement.cpp
double FootPlacement::Trajectory(double phase, double des1, double des2); //z-direction posture trajectory
//phase：the phase when reaching the highest
//des1：the highest position along the trajectory
//des2：the final position of the trajectory

步态控制 (Gait control)

//GaitScheduler.h
double tSwing; //the time of one step
double FzThrehold; //the maximal force when touching the ground

//GaitScheduler.cpp
DataBus::LegState legState=DataBus::RSt; //the first step state

关节参数 (Joint parameter)

//JointCtrConfig.json
"Joint-ankle-l-pitch" : {
    "PVT_LPF_Fc" : 20,
    "kd" : 5.0,
    "kp" : 50.0,
    "maxPos" : 0.61087,
    "maxSpeed" : 48.8,
    "maxTorque" : 58.5,
    "minPos" : -0.43644
}

更换机器人模型说明

模型文件

1. XML文件准备

准备机器人的urdf（.urdf）文件和mesh文件（.stl），用于添加mujoco编译标签

<mujoco>
<compiler
        meshdir="meshes/"
        balanceinertia="true"
        discardvisual="false" />
</mujoco>

将工作目录更改为mujoco-3.x.x/bin，运行命令：

./simulate

将urdf文件拖入仿真界面，模型显示正确后保存xml文件。您应该记下网格文件的路径。

您还可以参考 Mujoco官方文档来设置 compiler, option 或者 asset 等标签连来设置 body, actuator 和 sensor 等。

2. 更换型号

以“青龙机器人”为例，base_link 下有4个并联的串联连接：头部 Link_head、腰部 Link_waist、左臂 Linkarm_l、右臂 Link_arm_r 分支。左臂和右臂分支各有 7 个自由度，头部分支有 2 个自由度。腰部分支有3个自由度包括 pitchLink_waist_pitch、rollLink_waist_roll、yawLink_waist_yaw等，左腿和右腿分支并联，每条腿依次连接3个髋关节 Link_hip、1个膝关节 Link_knee、2个踝关节 Link_ankle 总共有 6 个自由度。这样就完成了全部31个自由度的配置。
您可以参考此配置并尝试自定义您的配置。

<worldbody>
    <body name="base_link" pos="x x x">
        <freejoint name="float_base" />
        <body name="body1" pos="x x x">
            <inertial pos="x x x" quat="x x x" mass="x" diaginertia="x x x"/>
            <joint name="joint1" pos="0 0 0" axis="1 0 0" limited="true" range="x x"/>
            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="x x x 1" mesh="body1"/>
            <geom type="mesh" rgba="x x x 1" mesh="body1"/>
            <body name="body2" pos="x x x">
                ...
                <joint name="joint2" .../>
                ...
            </body>
        </body>
        <body name="body3" pos="x x x">
            ...
            <joint name="joint3" .../>
            ...
        </body>
    </body>
</worldbody>

本例中，base_link下并联了两个分支，一个分支由body1和body2串联组成，另一个分支由body3组成。如果机器人有浮动底座，则在上面名为base_link的body下添加自由关节freejoint。如果机器人是固定底座，请拆下freejoint。或者，如果需要，可以在模型配置阶段屏蔽freejoint。

该项目为 31 个关节中的每一个关节设置执行器。

<actuator>
    <motor name="motor1"  joint="joint1" gear="x" ctrllimited="true" ctrlrange="x x"/>
    ...
</actuator>

用户可以根据机器人的自由度在活动关节处定义相应的执行器。

该项目配置了四元数framequat、测速仪velocimeter、角速度仪陀螺仪gyro、加速度计accelerometer等传感器，安装在body标签中已经定义的site，可以根据touch、force、torque、jointpos、jointvel、actuatorfrc、actuatorfrc的需要添加。可根据需要添加force、torque、jointpos、jointvel、actuatorfrc等传感器。

<sensor>
    <framequat name="xx" objtype="site" objname="imu" />
    <velocimeter name="xx" site="imu" />
    <gyro name="xx" site="imu" />
    <accelerometer name="xx" site="imu" />
</sensor>

除了自由度配置、执行器配置、传感器配置之外，其他更具体的参数修改可以参考Mojoco官方文档。

控制代码和Mujoco接口

使用函数 mj_loadXML、mj_makeData 获取 mjModel、mjData 结构体。您可以参考文档了解 mjModel、mjData、mjOption 的更多详细信息。

mjModel* mj_model = mj_loadXML("../Models/xxx.xml", 0, error, 1000);
mjData* mj_data = mj_makeData(mj_model);

mj_model->nv 是广义速度坐标的维数，即浮动底座的线速度、角速度、旋转型31个关节的速度。项目程序框架中与自由度相关的变量对应于mj_model->nv-6，动力学库会根据URDF自动获取机器人自由度的尺寸，其中定义了所有尺寸信息。这样用户就不用在程序中手动修改了。

由于本项目对body、joint、motor等部件地址的访问依赖于查询名称字符串并锁定地址，因此当修改某个部件时，不会影响其他body、joint的数据读写，相比直接索引编号为修改模型提供了便利。修改模型中某自由度控制参数时，只需修改MJ_Interface.h的JointName、Pin_KinDyn.h的motorName、PVT_Ctr.h的motorName以及JointCtrConfig.json文件中某自由度名称对应的变量即可。例如，要修改J_waist_pitch的刚度，需要修改JointCtrConfig.json中的J_waist_pitch和对应的PD参数，J_waist_pitch的名称对应xml文件中的joint名称、motor名称。

传感器数据地址也是通过查询名称字符串找到地址来访问的，添加或删除传感器可以通过修改MJ_Interface.h中相应的传感器名称来完成。

缩写表：
 

控制框架