基于QP优化的双轮腿–单臂机器人全身运动规划

Whole-Body Motion Planning for a Two-Wheeled-Legged Single-Arm Robot: A QP-Based Approach

摘要: 针对双轮腿–单臂机器人在全身运动规划中的自由度冗余问题，本文提出了一种基于二次规划(QP)优化的运动规划方法。该方法将躯干运动视为机械臂运动链的延伸，将手臂与躯干作为一个整体运动系统进行建模与控制。以TITA-PiPER机器人为研究对象，首先，通过建立机械臂末端位姿与七维广义关节速度之间的雅可比矩阵映射，并将该映射关系作为硬约束，以确保手臂末端任务具有最高优先级。然后，构造以关节运动平滑性为优化目标，同时融合关节限位、非完整约束及动态平衡稳定性的二次规划问题，从而统一求解出满足整体运动要求的七维广义关节加速度。仿真结果表明，该方法能够在保证手臂末端任务精确执行的同时，充分利用躯干的运动能力实现全身协调，为轮腿臂机器人的实时运动控制提供了有效的解决方案。

Abstract: In addressing the redundancy of degrees of freedom in whole-body motion planning for a two-wheeled-legged single-arm robot, this paper proposes a motion planning method based on Quadratic Programming (QP) optimization. This approach treats the torso motion as an extension of the robotic arm’s kinematic chain, modeling and controlling the arm and torso as an integrated motion system. Taking the TITA-PiPER robot as the subject of study, a Jacobian matrix mapping between the end-effector pose of the arm and the velocities of the seven-dimensional generalized joints is first established. This mapping is enforced as a hard constraint to guarantee the highest priority for the end-effector task. Subsequently, a QP problem is formulated. It minimizes joint motion jerk (or another smoothness metric) as the optimization objective while incorporating constraints such as joint limits, nonholonomic constraints, and dynamic balance stability. This framework allows for the unified solving of the seven-dimensional generalized joint accelerations that satisfy the overall motion requirements. Simulation results demonstrate that the proposed method ensures precise execution of the end-effector task while fully utilizing the torso’s mobility to achieve whole-body coordination. It provides an effective solution for the real-time motion control of wheeled-legged robots with an arm.

1. 引言

近年来，移动机器人技术在诸多领域取得了显著进展，逐渐应用于勘探、巡检、搬运等复杂任务中[1]。各类机器人在不同场景中展现出各自优势：轮式机器人在平坦地形上具有较高的移动效率和${\Sigma }_w$ 稳定性[2]-[4]，但越障与地形适应能力有限[5]；腿足式机器人擅长在非结构化环境中通行，具备较强的地形适应能力[6]-[9]，但能耗较高、运动不连续[10]。双轮腿机器人兼具轮式和足式机器人的优点，成为了移动机器人领域的一个重要研究方向。典型成果包括苏黎世联邦理工学院(ETH Zurich)的Klemm V等人为小型双轮腿机器人Ascento设计了辅助LQR全身控制方法[11]。该控制方法的核心为多任务规划部分，将各种运动任务，包括躯干站高、横滚、航向任务以及LQR任务(平衡任务)作为约束条件，规划一组关节期望加速度。爱丁堡大学的Xin S等人对双轮腿机器人的规划方法[12]是基于简化的小车线性倒立摆模型构造模型预测控制器，以质心参考速度为输入，规划满足ZMP判据的轮地接触点以及CoM位置。执行层基于全身动力学模型将关节运动映射为关节力矩。在仿真环境中实现了轮式运动加减速与行走运动跨越障碍的实验验证。南方科技大学研制的双轮腿机器人NeZha [13]站高约89 cm，采用基于轮式弹簧负载倒立摆(W‑SLIP)模型并结合二次规划的轨迹生成方法，通过分层控制框架实现了行进跳跃的仿真验证。印度理工学院(IIT Kharagpur)的Bhavanibhatla K等人对六足移动机械臂的在线全身运动规划方法[14]是基于非线性约束优化进行冗余分解，通过实时求解一个以最小化关节变量加权变化为目标、同时满足末端位置精度与可操作性约束的优化问题，来协调规划机械臂关节与移动平台的运动。该方法结合自适应步长策略动态调整躯干运动步长，以保证机械臂始终远离奇异位形，最终在仿真与实物平台上实现了用户通过操纵杆实时操控末端轨迹的无奇异全身协调运动。

在总结现有双轮腿机器人及其复合系统运动规划方法的基础上，针对双轮腿–单臂机器人在全身协调运动规划中存在的自由度冗余与多任务耦合问题，本文提出了一种基于末端任务硬约束与二次规划(QP)实时优化的全身运动规划方法。该方法将躯干与机械臂视为统一运动系统，将机械臂末端任务以雅可比矩阵等式约束形式作为最高优先级，从而将规划问题构建为包含运动学、动力学与执行器约束的统一QP优化模型。首先，基于雅可比映射构建末端任务的硬约束，确保其在任意工况下得到优先满足。在此基础上，以关节运动平滑性与能耗最小化为优化目标，系统考虑关节限位、非完整约束、动态平衡连续性以及执行器能力等多类约束，构建实时QP问题并在线求解，同步生成躯干与机械臂的协调运动加速度指令。本规划框架在保障末端任务精度的同时，实现了全身运动的动态平衡与实时可行，为双轮腿–单臂机器人在复杂作业场景中的运动规划提供了一套系统的解决方案。

本研究的创新点主要体现在以下两个方面：

1) 提出了基于末端任务硬约束的全身运动规划架构，将机械臂末端跟踪任务以最高优先级形式嵌入QP优化框架，实现了任务空间精度与系统运动冗余的统一处理。

2) 设计了融合多类物理约束的实时QP规划器，在统一优化模型中同时考虑非完整约束、关节限位、平衡连续性等实际限制，有效保障了运动可行性与动态稳定性。

2. 机器人系统介绍

本文以TITA-PiPER双轮腿–单臂机器人为研究对象，其三维模型如图1所示。该平台由本末科技(DirectDrive Tech)研发的TITA双轮腿机器人与松灵科技(AgileX Robotics)研发的6自由度轻量级机械臂PiPER集成构成。其中，TITA机器人的单侧轮腿结构包含四个自由度，分别由髋外摆关节、髋俯仰关节、膝俯仰关节以及轮旋转关节提供，关节电机全部由一体化准直驱动模组驱动，单关节峰值扭矩可达120 Nm，具备高动态响应能力。PiPER机械臂则具备六个自由度，由六个集成关节电机独立驱动，总重量为4.2 kg，额定负载能力为1.5 kg。其他部分参数如表1和表2所示。

Figure 1. 3D model of the robot

图1. 机器人3D图形

Table 1. Robot link parameters

表1. 机器人杆件参数

Component	Parameter Combinations (units)	Instructions
Torso	500 × 310 × 300~490 mm | 13.2 kg	L × W × H/Mass
Leg	Thigh: 200 mm/3.1 kg | Crus: 200 mm/0.6 kg	Length/Mass
Wheel	Radius 92.7 mm | Mass 1.5 kg	—
Arm Base	Height 80 mm	—
Upper Arm	Length 285 mm | Mass 0.2 kg	—
Forearm	Length 215 mm | Mass 0.5 kg	—
Gripper	0~70 mm | 40 N | 0.5 kg	Dynamic Range/Force/Mass

Table 2. Motor parameters

表2. 电机参数

Motor Name	Maximum Torque	Joint Range
Hip-outside motor	120 Nm	[−0.78, 0.78] rad
Hip-pitch motor	120 Nm	[−3.1, 1.6] rad
Knee motor	120 Nm	[−2.7, −0.69] rad
Wheel motor	48 Nm	—
J1 motor	—	[−2.69, 2.69] rad
J2 motor	—	[0, 3.40] rad
J3 motor	—	[−3.05, 0] rad
J4 motor	—	[−1.75, 1.95] rad
J5 motor	—	[−1.31, 1.31] rad
J6 motor	—	[−1.75, 1.75] rad

3. 目前问题和机械臂建模

双轮腿–臂机器人系统具有较高的运动自由度，在全身运动规划中存在冗余问题。为实现手臂末端轨迹的精确跟踪，同时协调躯干运动，本文提出一种基于二次规划(QP)的优化方法。该方法将手臂末端位置速度与七维广义关节速度之间的雅可比矩阵映射关系作为硬约束，以确保手臂末端任务具有最高优先级，从而规划满足多种约束的广义关节加速度。

坐标系如图1所示的，为世界坐标系，为固定参考系。${\Sigma }_a$ 为手臂夹爪坐标系原点位于夹爪中心，x轴指向夹爪的正前方，z轴指向夹爪正上方；躯干坐标系${\Sigma }_b$ 的原点位于躯干几何中心处，x轴指向躯干刚体的正前方，z轴指向躯干刚体的正上方；${\Sigma }_I$ 为支撑点坐标，${\Sigma }_I$ 的原点是轮地接触点之间的连接线的中点，x轴指向轮子的前进方向，z轴指向重力的相反方向，用右手定则确定y轴方向。

机械臂具有一个航向自由度，两个俯仰自由度。手臂关节空间定义为$q_a={\left[{\theta }_{a0},{\theta }_{a1},{\theta }_{a2}\right]}^{\text{T}}$ ，分别为肩部航向关节(j1)旋转角度、肩部俯仰关节(j2)角度、肘部俯仰关节(j3)角度(本研究聚焦于与躯干运动动力学耦合最为显著的肩部航向与俯仰关节，上述三个关节对躯干位姿调整及整体运动耦合的影响最为显著，故在仿真中仅使用这三个关节进行建模与分析；其余关节主要用于末端执行器姿态的精细调整，对躯干及系统整体的动力学耦合影响较小，因此在本文的动力学分析与运动规划中予以简化)。由于机械臂以躯干为动基座，因此，其初始运动状态空间为躯干的运动位姿$q_b={\left[x_b,y_b,z_b,\alpha \right]}^{\text{T}}$ (将躯干俯仰角$\beta$ 与横滚角$\gamma$ 近似为0)，其运动学分析基于躯干坐标系${\Sigma }_b$ 进行。

手臂末端位置在躯干坐标系中表示为${}^{b}p{}_h={\left[\begin{array}{lll}{x}_a\hfill & y_a\hfill & z_a\hfill \end{array}\right]}^{\text{T}}$ ，此时，手臂末端位置在世界坐标系${\Sigma }_w$ 中的描述可由下式计算得到：

${}^{w}p{}_h={}^{w}p{}_b+{}_b{}^{w}R{}^{b}p{}_h$ (1)

其中，${ }_b^{w}R=\left[\begin{array}{ccc}{c}_{\alpha }& -s_{\alpha }& 0\\ s_{\alpha }& c_{\alpha }& 0\\ 0& 0& 1\end{array}\right]$ ，${}^{w}p{}_b={\left[\begin{array}{lll}{x}_b\hfill & y_b\hfill & z_b\hfill \end{array}\right]}^{\text{T}}$ 。定义广义坐标$q={\left[q_b,q_a\right]}^{\text{T}}$ ，将公式(1)中的每一行对广义坐标的每一元素求偏导，得到雅可比矩阵：

$J_a={\left[\begin{array}{llll}\frac{\partial {}^{w}P{}_h}{x_b}\hfill & \frac{\partial {}^{w}P{}_h}{y_b}\hfill & \cdots \hfill & \frac{\partial {}^{w}P{}_h}{{\theta }_{a2}}\hfill \end{array}\right]}^{\text{T}}$ (2)

则手臂末端速度在世界坐标系中可表示为：

${}^w\dot{P}{}_h=J_a\dot{q}$ (3)

从而建立起手臂末端坐标${}^{w}P{}_h$ 与广义关节空间$q={\left[\begin{array}{ll}{q}_b\hfill & q_a\hfill \end{array}\right]}^{\text{T}}$ 间的速度映射关系。

4. 规划器

为使手臂末端到达期望位置${}^{w}P{}_h^d={\left[\begin{array}{ccc}{x}_a^d& y_a^d& z_a^d\end{array}\right]}^{\text{T}}$ ，需要规划一条连续的运动轨迹$q={\left[q_b,q_a\right]}^{\text{T}}$ ，包含躯干动基座的位姿与手臂关节角度值。双轮腿–臂机器人的运动自由度为7，用以进行手臂末端3自由度的调节，存在多条运动轨迹到达目标位姿。因此，将该运动规划转化为优化问题，由运动轨迹集合中定位一组满足等式与不等式约束的轨迹作为控制的输入。

4.1. 目标函数构造

现有相关论文中，一般将手臂末端位姿控制任务作为约束加入最优运动规划，该种处理方法存在的问题为，当给定值超出机器人执行范围，即打破结构约束才能实现精确执行时，该优化问题将不存在可行解空间，导致无解。鉴于此，本文将手臂末端位姿控制构造为目标函数，在机器人机械结构与运动能力允许范围内，使得手臂末端位姿与速度最大限度伺服到期望位姿与速度。

$\underset{{\ddot{q}}_{ref}}{\text{min}}f\left(x\right)={\left[{}^w\ddot{P}{}_h^{ref}-{}^w\ddot{P}{}_h\right]}^{\text{T}}Q\left[{}^w\ddot{P}{}_h^{ref}-{}^w\ddot{P}{}_h\right]$ (4)

其中，${}^w\ddot{P}{}_h^{ref}$ 为手臂末端期望参考加速度，通过比例–微分控制器得到

${}^w\ddot{P}{}_h^{ref}={}^w\ddot{P}{}_h^d+K_p\left({}^{w}P{}_h^d-{}^{w}P{}_h\right)+K_d\left({}^w\dot{P}{}_h^d-{}^w\dot{P}{}_h\right)$ (5)

${}^w\ddot{P}{}_h$ 为手臂末端当前加速度，${}^w\ddot{P}{}_h=J_a\ddot{q}+{\dot{J}}_a\dot{q}$ 。

由于机器人的冗余特性，仅以公式(5)为目标函数，并不能唯一地确定躯干与手臂关节运动，从而使得手臂末端位置与速度尽可能达到${\left[\begin{array}{cc}{}^{w}P{}_h^d& {}^w\dot{P}{}_h^d\end{array}\right]}^{\text{T}}$。因此，将广义关节空间的运动位置增量进行加权平方，加入目标函数，

$\underset{{\ddot{q}}_{ref}}{\min }f\left(x\right)={\left[{}^w\ddot{P}{}_h^{ref}-{}^w\ddot{P}{}_h\right]}^{\text{T}}Q\left[{}^w\ddot{P}{}_h^{ref}-{}^w\ddot{P}{}_h\right]+{\left[q-q_{pre}\right]}^{\text{T}}R\left[q-q_{pre}\right]$ (6)

其中，$q$ 为本次控制率施加到机器人后产生的广义关节空间位置，通过如下式的离散预测得到

$q=q_{pre}+\left({\dot{q}}_{pre}+\overline{q}\Delta t\right)\Delta t$ (7)

其中，$q_{pre}$ 与${\dot{q}}_{pre}$ 采用传感器测得。

至此，目标函数的构造同时考虑了手臂末端任务及关节空间运动的优化目标，同时，机器人自身的关节空间约束、执行器性能约束以及非完整约束等必须严格满足，因此，进行等式约束与非等式约束构造。

4.2. 约束构造

为将手臂末端任务作为最高优先级，建立以下基于雅可比矩阵映射的等式约束，确保末端加速度任务精确执行：

$J_a\ddot{q}=b$ (8)

其中：

$b={}^w\ddot{P}{}_h^{ref}-{\dot{J}}_a\dot{q}$ (9)

${}^w\ddot{P}{}_h^{ref}={}^w\ddot{P}{}_h^d+K_p\left({}^{w}P{}_h^d-{}^{w}P{}_h\right)+K_d\left({}^w\dot{P}{}_h^d-{}^w\dot{P}{}_h\right)$ (10)

$b$ 表示期望的末端加速度${}^w\ddot{P}{}_h^{ref}$ 减去由当前关节速度$\dot{q}$ 通过雅可比时间导数${\dot{J}}_a$ 引起的末端加速度分量(即科氏加速度和向心加速度)。因此，$b$ 代表了需要由关节加速度$\ddot{q}$ 直接贡献的末端加速度部分，即关节加速度应产生的末端加速度分量。

为约束广义关节空间的变量不超出上下界限，设置如下式的双边不等式约束

$\underset{\_}{q}\le q_{pre}+\left({\dot{q}}_{pre}+\ddot{q}\Delta t\right)\Delta t\le \overline{q}$ (11)

其中，$\underset{\_}{q}$ 与$\overline{q}$ 分别为广义关节空间的位置下界与上界。

广义关节空间中包含了躯干的三维运动$x_b$ 、$y_b$ 、$z_b$ 以及姿态$\alpha$ ，另外，默认躯干的期望俯仰角与期望横滚角为0，不需要规划器实时生成。机器人无法直接发生侧向移动，只能通过控制前向运动与航向的耦合来实现侧向运动。因此，当规划器进行躯干位姿实时规划时，添加约束以保证位姿间的耦合关系：

$\frac{x}{\cos \alpha }-\frac{y}{\sin \alpha }=0$ (12)

其中，$\left[\begin{array}{c}x\\ y\\ \alpha \end{array}\right]=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0\end{array}\right]\left(q_{pre}+\left({\dot{q}}_{pre}+\ddot{q}\Delta t\right)\Delta t\right)$ 。

同时，由于双轮腿需要保持实时动态稳定，受到外界环境的突发变化，机器人的平衡状态将由当前平衡点跳变为另一个不连续的动态稳定平衡点，为了保证转换过程的连续平滑，构造加速度变化量的约束：

$-\epsilon \le \left[\begin{array}{cc}{E}_{4\times 4}& 0_{4\times 3}\end{array}\right]\left(\ddot{q}-{\ddot{q}}_{pre}\right)\le \epsilon$ (13)

其中，以公式(6)为目标函数，以公式(8)、(11)、(12)和(13)为约束条件便可优化求解出以手臂末端位置为任务空间的广义关节加速度期望轨迹。

5. 仿真

为验证所提出规划方法的有效性，评估双轮腿–臂机器人在执行大范围伸展任务时的运动精度与全身协调稳定性，本研究在Webots仿真平台上构建仿真环境，对TITA-PiPER机器人进行物理建模，并使用C++语言编写基于QP优化的实时控制程序。本次仿真实验的工况设置为：机械臂末端在世界坐标系中由初始位置沿x方向伸展至2.0 m，同时沿z方向伸展至0.8 m，旨在测试系统在较大工作空间内进行轨迹跟踪时的整体性能。

(a)

(b)

Figure 2. Simulation images and state curves

图2. 仿真图像和状态曲线

仿真过程中，机器人以动态平衡状态启动，通过第4节所述的QP规划器实时计算广义关节加速度指令，其中手臂末端轨迹跟踪任务以最高优先级的硬约束形式保证。图2(a)展示了机械臂末端从初始位置运动到目标位置(x = 2.0 m, z = 0.8 m)的连续运动过程。同时，躯干在手臂运动过程中通过优化自主调整其位姿以辅助完成任务，并维持自身平衡。

仿真结果如图2(b)所示。图2(b)的第一、二幅图为机械臂末端在x和z方向上的位置跟踪曲线，其中绿色实线为期望轨迹，红色虚线为实际轨迹。可以看出，实际轨迹能紧密跟随期望轨迹，其稳态跟踪误差在x和z方向上均保持在±0.001 m以内，验证了所提方法在任务空间控制上的高精度。图2(b)的第三幅图显示了躯干在x方向上的速度响应。在机械臂大范围伸展过程中，躯干速度受到耦合动力学影响而产生变化，但其速度误差始终被控制在±0.05 m/s的范围内，表明优化器有效协调了躯干运动以满足末端约束。图2(b)第四幅图展示了躯干姿态角的变化情况。在机器人稳定运动过程中，幅值均小于0.0025 rad，说明系统在完成伸展任务的同时，很好地维持了躯干的姿态稳定，避免了因手臂运动引发的姿态失稳。

6. 结论

本文针对双轮腿–单臂机器人在全身运动规划中存在的自由度冗余与多任务协调问题，提出了一种基于末端任务硬约束与二次规划(QP)实时优化的全身运动规划方法。该方法将机械臂末端跟踪任务设定为最高优先级，通过建立其与七维广义关节加速度间的雅可比矩阵等式约束，确保了任务空间控制的精确性。在此硬约束基础上，构建了以关节运动平滑性为优化目标，并融合关节限位、非完整约束及动态平衡连续性等多类物理约束的QP问题，实现了躯干与机械臂加速度的同步协调规划。