变高度双轮足平台自适应平衡控制算法设计

Design of Adaptive Self-balancing Control Algorithm for Variable Height Dual-wheel-legged Platforms

摘要

轮足式平台在腿部结构末端安装驱动轮，兼具铺装路面的高通过性和越野路面的高机动性，平台变高度时重心位置的变化对稳定性影响较大。为增强变高度自适应能力，建立平台的动力学模型，并在平衡点处作线性化处理。根据线性化状态方程特点，基于积分控制原理扩张状态变量，建立了扩张后的状态空间模型。采用线性二次型调节器分层控制方法获得反馈控制律，搭建了仿真模型，试制了物理样机，开展了仿真及实验研究。结果表明，提出的反馈控制率能够很好地实现平台的自平衡，扩张的状态变量也能很好地平衡重心位置的变化，实现平台变高度自适应平衡控制。

Abstract

A two wheel platform was equipped with drive wheels at the end of the leg structure， which had both high passability on paved roads and high maneuverability on off-road roads. In order to enhance the adaptive ability of the platform， the dynamics model of the platform was established， and linearized at the equilibrium point， according to the characteristics of the linearized equation of state， the state variables were expanded based on the principle of integral control， and the expanded state space model was established. The LQR （linear quadratic regulator）method was used to obtain the feedback control law， the simulation model was built， the physical prototype was trial-produced， and the simulation and experimental research were carried out. The results show that the proposed feedback control rate may well realize the self-balancing of the platform， and the expanded state variable may also well balance the change of the center of gravity position， and realize the adaptive equilibrium control of the platform variable heights.

Graphical abstract

关键词

双轮足平台 / 可变高度 / 积分控制 / 自平衡算法

Key words

dual-wheel-legged platform / variable height / integration control / self-balancing algorithm

Author summay

张雷^*（通信作者），男，1987年生，博士、副教授。研究方向为无人平台建模与控制。E-mail：[e-mail]。

0 引言

轮足式平台是指在腿式平台的足端安装驱动轮的机构，该机构既保留了腿式平台高越障能力，又大大增强了其机动性。双轮足平台结构简单，机动灵活，但稳定性较差，需要专门设计的算法来控制平台的自稳定性。特别是双轮足平台高度改变会引起其质心位置的变化，导致系统平衡状态及平衡特性发生改变，需要深入研究。WANG等^［1］利用车轮的地面黏性阻尼平衡了部分质心偏移。张弨 ^［2］设计速度环PI控制器实现了平台对质心变化的自适应。VICTOR等^［3］设计的Ascento采用了特殊设计的腿部机构，使得质心轨迹近似为直线，大幅减小了质心偏移对平台稳定性的影响。YANG等^［4］设计了具有新型五杆并联机构的多模轮式双足机器人SUSTech-Nezha Ⅱ，采用基于线性二次型调节器（linear quadratic regulator， LQR）的分层控制方法对其进行运动控制。YU等^［5］设计了双轮足机器人HotWheel，其单腿具备四个自由度，增加了侧摆关节，采用虚拟模型控制概念设计了一种分层的模型预测控制（model predictive control，MPC）位姿跟踪控制器和位姿估计器，使得机器人可以稳定在V形斜坡上。WANG等^［6］针对robot Ollie设计了Luenberger 观测器用于确定质心侧偏角，实现了平衡点附近参数的快速估计。ZHANG 等^［7］采用数据驱动的方法实现了部分参数未知情况下的自平衡控制，获得了很好的控制效果。单开正等^［8］采用共轴近似直驱结构方案实现了平台的近似直驱，大大减小了质心偏移角的影响。还有一些研究采用自适应控制算法来应对质心偏差带来的稳定性变差问题，包括神经网络控制器、自适应模糊控制器、遗传算法控制器，但这些算法对控制器的性能要求较高^［9-13］。

双轮足平台质心侧偏角度对稳定有决定性影响，为克服变高度过程中质心变化带来的扰动，本文基于积分控制原理扩张了状态量，建立了双轮足平台的动力学方程，并在平衡点附近对其线性化，建立了扩张后的状态方程，利用LQR方法实现了反馈控制。为验证算法的有效性，基于Simulink搭建了仿真实验环境并试制了物理样机，开展了双轮足平台定高度和变高度自平衡仿真和实验研究，通过实验结果验证了算法的可行性和仿真环境的可靠性。在此基础上，利用仿真实验环境开展了双轮足平台特性仿真分析，讨论了一些特殊工况下的系统平衡特性。

1 变高度双轮足平台数学建模

1.1 变高度双轮足平台结构简介

图1所示的双轮足平台由车架、腿部机构及轮毂电机组成。其中，腿部机构采用四连杆机构，由大臂、小臂以及连杆组成，为单自由度机构，由关节电机驱动，可动态改变质心相对于轮毂电机的距离和位置。车架上安装有控制系统、轮毂电机驱动器、惯性测量单元（IMU）以及电池等。平台的俯仰角度及角速度由IMU提供，当平台处于水平位置时，俯仰角为零。平台轮毂电机角速度由轮毂电机控制器提供，关节角位移由关节电机驱动器提供 ^［11］。

轮足式平台可简化成杆长和转动惯量随关节电机转角变化的单轮倒立摆动结构，为建立其动力学模型，需知车架及腿部机构整体质心属性，首先需建立其运动学模型。

1.2 变高度双轮足平台运动学建模

为简化模型，在不显著影响建模精度的前提下对可变高度双轮足无人平台作如下假设：①双轮足平台高度变化足够缓慢，可忽略动载荷效应；②左右臂腿部机构位移相等，认为横滚角位移为零；③连杆质量很小，建模时可忽略。

基于假设，将车架及腿部机构看成一个整体，称为车体，其质心用M表示，车体相对于轮毂电机中心位置由关节电机转角决定，如图 2所示。

当车架水平时，车轮中心到车体质心M连线与铅垂线方向的夹角定义为初始质心俯仰角，用{\beta }_{\mathrm{0}}表示。IMU输出俯仰角为\beta时，实际质心俯仰角为

\delta =\beta -{\beta }_{\mathrm{0}}（1）

基于上述简化，车体质心M相对于轮毂电机的位置及转动惯量是关节电机转角的函数，需要建立腿部机构运动学来进行计算。建立的坐标系如图3所示，描述如下。

坐标系o₀x₀y₀z₀位于车架左右关节连线的中心位置，x₀方向平行于车头方向，z₀方向垂直于大臂，指向车体左侧。

坐标系o₁x₁y₁z₁位于大臂与车架铰接关节处，x₁方向沿大臂轴线方向，z₁平行于z₀方向。坐标系o₂x₂y₂z₂位于大臂与小臂铰接关节处，x₂方向沿小臂轴线方向，z₂方向平行于z₀方向。坐标系o₃x₃y₃z₃位于小臂与轮毂电机铰接关节处，x₃方向水平向前，z₃方向平行于z₀方向。

大臂轴线与车架坐标系o₀x₀y₀z₀的x₀方向夹角为θ₁，小臂轴线与大臂坐标系o₁x₁y₁z₁的x₁轴夹角为θ₂。由于受连杆的约束，θ₁与θ₂之间满足一定的数学关系。

若大臂的质心用p₄表示，其质心位置D-H坐标为 r₄，那么p₄在坐标系o₀x₀y₀z₀中的表示为

{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{}_{\mathrm{4}}^{\mathrm{0}}{=}_{\mathrm{1}}^{\mathrm{0}}\mathit{X}\cdot {\mathit{r}}_{\mathrm{4}}（2）

式中：{}_{\mathrm{1}}^{\mathrm{0}}\mathit{X}为坐标系o₁x₁y₁z₁向坐标系o₀x₀y₀z₀变换的矩阵。

同样，小臂的质心位置p₅在坐标系o₀x₀y₀z₀中表示为

{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{}_{\mathrm{2}}^{\mathrm{0}}{=}_{\mathrm{2}}^{\mathrm{0}}\mathit{X}\cdot {\mathit{r}}_{\mathrm{5}}（3）

式中：{}_{\mathrm{2}}^{\mathrm{0}}\mathit{X}为坐标系o₂x₂y₂z₂向坐标系o₀x₀y₀z₀变换的矩阵； r₅为小臂质心D-H坐标。

在o₀x₀y₀z₀中，车体质心点位置可以表示为

{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{}_M^{\mathrm{0}}=\frac{\mathrm{2}{m}_{\mathrm{4}}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{\mathit{r}}_{\mathrm{4}}^{\mathrm{3}}+\mathrm{2}{m}_{\mathrm{5}}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{\mathit{r}}_{\mathrm{5}}^{\mathrm{3}}+m_{\mathrm{0}}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{\mathit{r}}_{\mathrm{0}}^{\mathrm{3}}}{\mathrm{2}{m}_{\mathrm{4}}+\mathrm{2}{m}_{\mathrm{5}}+m_{\mathrm{0}}}（4）

式中：m₀、m₄、m₅分别为机架、大臂和小臂的质量；{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{}_{\mathrm{4}}^{\mathrm{3}}表示大臂质心相对于车轮质心的矢量在坐标系o₀x₀y₀z₀中的位置；{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{}_{\mathrm{5}}^{\mathrm{3}}表示小臂质心相对于车轮质心的矢量在坐标系o₀x₀y₀z₀中的位置；{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{}_{\mathrm{0}}^{\mathrm{3}}表示车架质心相对于车轮质心在坐标系中的位置。

那么车体质心M到车轮的距离l可以表示为

l=\Vert {}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}\mathit{r}{}_M^{\mathrm{0}}-{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}\mathit{r}{}_{\mathrm{w}}^{\mathrm{0}}\Vert（5）

式中：{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{}_{\mathrm{w}}^{\mathrm{0}}为车轮质心位置在坐标系o₀x₀y₀z₀中的表示。

车体质心M的转动惯量在坐标系o₃x₃y₃z₃中可以表示如下：

{}_{}{}^{o_{\mathrm{3}}{x}_{\mathrm{3}}{y}_{\mathrm{3}}{z}_{\mathrm{3}}}{}_M^{\mathrm{3}}=\mathrm{2}[I_{\mathrm{4}}+m_{\mathrm{4}}({}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}\mathit{r}{}_{\mathrm{4}}^{\mathrm{0}}-{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}\mathit{r}{}_{\mathrm{w}}^{\mathrm{0}}{)}^{\mathrm{T}}·

({}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}\mathit{r}{}_{\mathrm{4}}^{\mathrm{0}}-{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}\mathit{r}{}_{\mathrm{w}}^{\mathrm{0}})]+\mathrm{2}[I_{\mathrm{5}}+m_{\mathrm{5}}({}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}\mathit{r}{}_{\mathrm{5}}^{\mathrm{0}}-{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}\mathit{r}{}_{\mathrm{w}}^{\mathrm{0}}{)}^{\mathrm{T}}·({}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}\mathit{r}{}_{\mathrm{5}}^{\mathrm{0}}-{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}\mathit{r}{}_{\mathrm{w}}^{\mathrm{0}})]+I_{\mathrm{0}}+m_{\mathrm{0}}{(}_{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{\mathit{r}}_{\mathrm{w}}^{\mathrm{0}})·{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}\mathit{r}{}_{\mathrm{w}}^{\mathrm{0}}（6）

式中：I₀、I₄、I₅分别为机架、大臂和小臂在质心处绕o₀z₀轴的转动惯量。

图3中A点为连杆与小臂的铰接点，A点在坐标系o₀x₀y₀z₀中可以表示为

{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{}_A^{\mathrm{0}}{=}_{\mathrm{2}}^{\mathrm{0}}\mathit{X}\cdot {\mathit{r}}_A（7）

{\mathit{r}}_A=［-l₂-l₃ 0 0］

由于A、B（连杆与车架的铰接点）两点之间距离受连杆长度的约束，故有下式成立：

l=‖{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{\mathit{r}}_A^{\mathrm{0}}-{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}\mathit{r}{\mathit{r}}_B^{\mathrm{0}}‖（8）

{}_{}{}^{o_{\mathrm{0}}{x}_{\mathrm{0}}{y}_{\mathrm{0}}{z}_{\mathrm{0}}}{}_B^{\mathrm{0}}=［-l₀ 0 0］

平台腿部机构l₀～l₄尺寸如表1所示，位置如图3所示。平台各部件的质量参数如表2所示。

根据上述参数建立仿真模型，当双轮足平台关节电机转动角度θ₂从70°变化到120°时，车体质心M距轮毂电机的距离以及质心处转动惯量的变化如图4所示。

1.3 动力学建模

基于1.2节的假设，将双轮足无人平台简化为单轮倒立摆系统，如图 5所示。图中的杆长l及车体质心处的转动惯量随关节电机转角变化，可由1.2节中建立的运动学模型计算。

图5b所示为简化后的单轮倒立摆模型，r为轮毂电机的半径；τ轮毂电机的驱动力矩；f为地面作用在轮毂电机上摩擦力。

对于简化后的单轮倒立摆，y方向上满足力平衡，绕z轴方向满足力矩平衡。分析简化模型y方向上的受力平衡特性，得到如下方程：

f-r(M+m)\dot{\omega }-Ml\mathrm{c}\mathrm{o}\mathrm{s}(\beta +{\beta }_{\mathrm{0}})\ddot{\beta }+

Ml\mathrm{s}\mathrm{i}\mathrm{n}(\beta +{\beta }_{\mathrm{0}})\dot{\beta }=\mathrm{0}（9）

式中：m为轮毂电机的质量；M为车体等效质量；\dot{\omega }为轮毂电机的角加速度；{\beta }_{\mathrm{0}}为质心侧偏角。

质心M绕o点在z轴方向上满足力矩平衡方程：

Mgl\mathrm{s}\mathrm{i}\mathrm{n}(\beta +{\beta }_{\mathrm{0}})-(M{l}^{\mathrm{2}}+I_M)\ddot{\beta }-\tau -

Mlr\mathrm{c}\mathrm{o}\mathrm{s}(\beta +{\beta }_{\mathrm{0}})\dot{\omega }=\mathrm{0}（10）

式中：I_M为车体在质心处绕y轴的转动惯量；g为重力加速度。

轮毂电机输出力矩与地面反作用力之间存在如下关系：

\tau -I_M\dot{\omega }-fr=\mathrm{0}（11）

联立式（9）～式（11），消去f，可以解出\dot{\omega }及\ddot{\beta }，写作

\frac{\mathrm{d}}{\mathrm{d}t}\left[\begin{array}{c}\omega \\ \dot{\beta }\end{array}\right]=F(\omega ,\beta ,\dot{\beta },{\beta }_{\mathrm{0}})（12）

式（12）为描述双轮足平台动力特性的微分方程，在平衡点附近线性化以后可用于平衡控制。

2 双轮足平台平衡点线性化及状态扩张

2.1 平衡点线性化

选取状态变量\mathit{x}={(\omega ,\beta ,\dot{\beta })}^{\mathrm{T}}，系统的平衡点为{\mathit{x}}_{\mathrm{0}}=({\omega }_{\mathrm{r}\mathrm{e}\mathrm{f}},-{\beta }_{\mathrm{0}}{,\mathrm{0})}^{\mathrm{T}}，将系统平衡点附近线性化，可以得到系统线性化的状态描述方程如下：

\dot{\mathit{x}}=\mathit{A}(\mathit{x}-{\mathit{x}}_{\mathrm{0}})+\mathit{B}u（13）

\mathit{A}=\left[\begin{array}{ccc}\mathrm{0}& -\frac{M^{\mathrm{2}}g{l}^{\mathrm{2}}r}{{\sigma }_{\mathrm{1}}}& \mathrm{0}\\ \mathrm{0}& \mathrm{0}& \mathrm{1}\\ \mathrm{0}& \frac{gl{M}^{\mathrm{2}}{r}^{\mathrm{2}}+glmM{r}^{\mathrm{2}}+I_{\mathrm{w}}glM}{{\sigma }_{\mathrm{1}}}& \mathrm{0}\end{array}\right]

\mathit{B}=\left[\begin{array}{c}\frac{M{l}^{\mathrm{2}}+Mrl+I_M}{{\sigma }_{\mathrm{1}}}\\ \mathrm{0}\\ -\frac{I_{\mathrm{w}}+M{r}^{\mathrm{2}}+m{r}^{\mathrm{2}}+Mlr}{{\sigma }_{\mathrm{1}}}\end{array}\right]

{\sigma }_{\mathrm{1}}=I_M{I}_{\mathrm{w}}+I_{\mathrm{w}}M{l}^{\mathrm{2}}+I_{M}M{r}^{\mathrm{2}}+I_{M}m{r}^{\mathrm{2}}+M{l}^{\mathrm{2}}m{r}^{\mathrm{2}}

式中：u为反馈作用力矩；I_{\mathrm{w}}为轮毂电机在质心处的转动惯量。

得到双轮足平台平衡点附近线性化的描述方程以后，可以设计反馈控制率对模型进行反馈控制。

2.2 基于积分控制的状态扩张

对于双轮足平台，系统平衡时，车体质心与车轮中心的连线为铅垂线方向，即车体质心俯仰角为零，满足\beta =-{\beta }_{\mathrm{0}}。

若采用反馈控制实现平台的自平衡，反馈系数设置为\mathit{K}=\left[\begin{array}{ccc}{k}_{\mathrm{1}}& k_{\mathrm{2}}& k_{\mathrm{3}}\end{array}\right]，则反馈作用力矩表示为

u=-\mathit{K}\mathit{x}（14）

系统平衡时，满足\dot{\mathit{x}}=\mathrm{0}，将式（14）代入式（13），化简可得

\omega ={\omega }_{\mathrm{r}\mathrm{e}\mathrm{f}}-\frac{k_{\mathrm{2}}}{k_{\mathrm{1}}}{\beta }_{\mathrm{0}}（15）

式中：{\omega }_{\mathrm{r}\mathrm{e}\mathrm{f}}为轮毂电机要追踪的角速度。

式（15）表明采用状态反馈控制在系统平衡时，双轮足平台车轮的角速度不为零，为此引入积分控制，将车轮的角速度积分以后作为状态变量，引入到反馈控制回路中，实现平台的稳定控制。平台扩张的状态变量命名为\theta，有下式成立：

\dot{\theta }=\omega（16）

则系统新的状态变量为{\mathit{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}={(\theta ,\omega ,\beta ,\dot{\beta })}^{\mathrm{T}}，系统的状态方程可以描述为

{\dot{\mathit{x}}}_{\mathrm{n}\mathrm{e}\mathrm{w}}={\mathit{A}}_{\mathrm{n}\mathrm{e}\mathrm{w}}({\mathit{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}-{\mathit{x}}_{\mathrm{0}}^{\mathrm{\text{'}}})+{\mathit{B}}_{\mathrm{n}\mathrm{e}\mathrm{w}}u（17）

{\mathit{x}}_{\mathrm{0}}^{\text{'}}=\left[\begin{array}{c}\mathrm{0}\\ \mathit{x}\end{array}\right]{\mathit{A}}_{\mathrm{n}\mathrm{e}\mathrm{w}}=\left[\begin{array}{cc}\mathrm{1}& \mathrm{0}\\ \mathrm{0}& \mathit{A}\end{array}\right]{\mathit{B}}_{\mathrm{n}\mathrm{e}\mathrm{w}}=\left[\begin{array}{c}\mathrm{0}\\ \mathit{B}\end{array}\right]

2.3 基于LQR反馈控制算法

将双轮足无人平台状态扩张以后，在平衡点附近将模型线性化，获得平台扩张后的状态空间描述方程。可以采用LQR方法设计系统的反馈控制率，实现平台变高度自平衡稳定控制的目标。平台y方向预设的运动速度为v_ref，当前状态下的车架初始质心侧偏角为{\beta }_{\mathrm{d}\mathrm{e}\mathrm{s}}，那么系统平衡时的参考状态变量可以表示为{\mathit{x}}_{\mathrm{r}\mathrm{e}\mathrm{f}}={[\begin{array}{cccc}\mathrm{0}& v_{\mathrm{r}\mathrm{e}\mathrm{f}}/r& {\beta }_{\mathrm{d}\mathrm{e}\mathrm{s}}& \mathrm{0}\end{array}]}^{\mathrm{T}}。

系统调整过程中代价函数可以写作

J={\int }_{\mathrm{0}}^{\mathrm{\infty }}(\mathit{x}-{\mathit{x}}_{\mathrm{r}\mathrm{e}\mathrm{f}}{)}^{\mathrm{T}}\mathit{Q}(\mathit{x}-{\mathit{x}}_{\mathrm{r}\mathrm{e}\mathrm{f}})+\mathit{R}{u}^{\mathrm{2}}（18）

式中： Q 为4×4维的状态量权重矩阵； R 为1×1维作用力权重矩阵。

控制变量u可以写作

u=-\mathit{K}(\mathit{x}-{\mathit{x}}_{\mathrm{r}\mathrm{e}\mathrm{f}})（19）

3 算法验证及特性分析

3.1 双轮足平台仿真建模及样机实验

为验证算法是否有效，搭建了仿真模型并试制了物理样机，开展自平衡仿真及实验。选取较为合适的 Q 与 R 权重矩阵进行仿真。在自平衡控制和前向运动控制中，自平衡控制更为关键，直接影响系统的平衡性，在确定 Q 与 R 矩阵数值时，赋予自平衡相关系数更大的权重，分别取

\mathit{Q}=\left[\begin{array}{cccc}\mathrm{1}& \mathrm{0}& \mathrm{0}& \mathrm{0}\\ \mathrm{0}& \mathrm{10}& \mathrm{0}& \mathrm{0}\\ \mathrm{0}& \mathrm{0}& \mathrm{50}& \mathrm{0}\\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{50}\end{array}\right]R =［1］

基于Simulink/Simscape搭建双轮足无人平台仿真模型，将双轮足平台三维模型经过合理简化及合并后导入软件中，建立平台仿真模型。在关节位置施加约束，模拟关节运动，在左右两轮与地面之间施加接触力，以模拟车轮与地面之间的相互作用，在车轮及小臂关节处施加驱动力，以模拟关节电机驱动作用。接触力的刚度设置为1×10⁵ N/m、阻尼设置为1×10³ N·s/m、静摩擦因数和动摩擦因数分别设置为0.5和0.3，搭建的双轮足无人平台仿真模型如图6所示。

根据三维模型试制物理样机，腿部驱动轮采用中凌科技的轮毂电机，关节驱动采用海泰HT_N18关节电机（额定扭矩18 N·m，额定转速42 r/min）驱动小腿关节，车架俯仰角感知采用维特智能HWT901B传感器测量，平台控制采用搭载RK3588（8核，主频2.4G）的orangepi5B。

为了验证自适应平衡控制算法的正确性，开展了定高度自平衡实验及仿真。设置仿真时间为6ｓ，使用固定步长求解器，采用其显式常微分方程求解器ode45仿真求解。步长设置为0.002 s，设定膝关节夹角为40°，车体初始俯仰角为25°，使得车体重心大致位于车轮的上方。对车体的俯仰角、俯仰角速度以及车轮角速度进行分析，得到的结果如图7所示。

图7a所示为车体俯仰角随时间的变化关系，可以看出，初始位置时车体重心并没有位于车轮正上方，经过算法的控制，俯仰角约在2 s后趋于稳定，并且最终维持在这个位置。仿真结果表明算法能够快速调整俯仰角使其趋于稳定。图7b和图7c所示分别为车体俯仰角速度及轮毂电机角速度随时间的变化曲线，在本文算法的控制下，两者均趋向于零，说明在算法的作用下，双轮足平台能够在1~2 s的稳定时间内恢复到自平衡状态，在动态调整和恢复平衡方面表现良好。

图7a所示为实验和仿真条件下车体俯仰角调整过程，两者最终平衡的结果有差别，说明物理样机和仿真模型车架重心位置存在一定差别，通过优化布置物理样机的部件位置，使得两者重心位置趋于一致。但同时，上述实验结果表明，在实际环境的噪声和干扰影响下，算法仍能逐步趋于稳定，说明本文算法具有一定的鲁棒性。为了说明自平衡算法在平台变高度过程中效果，开展了膝关节转角分别为40.0°、63.0°、91.7°下的自平衡实验及仿真。样机不同高度自平衡结果如图8所示，其中车架俯仰角及轮毂电机的速度变化如图9所示。

实验中关节电机初始角度为40.0°，车架质心位于平衡点附近。在t=2 s时，关节电机角度调整为63.0°，平台在算法的控制下，在t=3.5 s时基本恢复到平衡状态，此时俯仰角由25.0°减小到18.6°。在t=4 s时，关节电机角度调整为91.7°，平台在算法的控制下，当t=6 s时，基本恢复到平衡状态，此时俯仰角由18.6°减小到14.3°。

观察实验结果，在t=2 s和t=4 s时，俯仰角和轮毂电机角速度由于关节电机的角度调整均出现明显波动，但算法能够在1~2 s内快速抑制这些波动，表明算法具有良好的收敛速度和动态响应能力。

从定高度及变高度实验及仿真结果对比来看，两者变化规律基本吻合，说明仿真模型具有较高的可信度，可以用来分析平台自适应算法在一些特殊工况中的控制效果。

3.2 基于仿真模型的双轮足无人平台自平衡特性

当平台初始俯仰角与平衡值差异较大时，平台启动过程会出现一个较大幅度的自平衡矫正过程。通常情况下，平台可以从车架水平状态启动，此时IMU输出俯仰角为0°，这种状态下平台自平衡能力是衡量控制算法综合性能的重要指标。为此，利用仿真模型进行初始俯仰角为零的自平衡过程、不同纵向速度下自平衡过程以及高度连续可变条件下自平衡过程的仿真实验，结果如图 10~图12所示。

图10所示为双轮足平台在初始俯仰角为零的情况下的自平衡过程，可以看出，在算法的控制下，平台在不到2 s的时间内实现了自平衡，俯仰角很快收敛到平衡态0.3403 rad，平台俯仰角速度以及轮毂电机角速度收敛到0，平台的扩张状态量也相应地收敛到-0.057 rad。

图11为平台纵向角速度响应曲线，给定轮毂电机的角速度指令，仿真双轮足平台跟踪角速度指令过程。从图中可以看出，在t=1.5 s时，轮毂电机的角速度指令跃升为5 rad/s，平台在t=2.8 s时已经能够很好地追踪该角速度指令；在t=3.5 s时，角速度指令跃升为15 rad/s，平台在t=5 s时已经能够很好地追踪该角速度指令。在角速度追踪平衡过程中，车架俯仰角基本维持不变。

双轮足平台变高度自平衡过程仿真结果如图12所示，在平台高度变化过程中，平台的俯仰角随之变化，经过短暂调整后，很快能收敛到平衡态，轮毂电机速度的变化也呈现类似的规律。

双轮足无人平台特殊工况下的自平衡仿真结果表明，文中提出的扩张状态量可以较好地平衡平台高度变换引起的质心位置变化，利用LQR方法对平台进行稳定控制可显著提高平台变高度自适应平衡能力。

4 结论

1）基于积分控制原理对双轮足平台状态进行扩张后，采用LQR获得反馈控制率可以有效实现平台的稳定控制。

2）扩张状态的引入可以很好地抑制双轮足平台因高度变化引起重心变化所带来的扰动，使得平台具备变高度自适应控制能力。

3）初始车架质心相对于轮毂电机中心位置对启动过程中平台的平衡特性有较大影响，通过合理设置初始质心位置可以显著提高平台启动过程的稳定性。

参考文献

[1] WANG S， CUI L， ZHANG J， et al. Balance Control of a Novel Wheel-legged Robot： Design and Experiment［C］∥ International Conference on Robotics and Automation. Xi'an， 2021：6782-6788.

[2] 张弨.双足轮腿机器人系统设计与运动控制研究［D］. 哈尔滨：哈尔滨工业大学， 2020. .

[3] ZHANG Zhao. Research on System Design and Locomotion Control of a Bipedal Leg-wheeled Robot［D］. Harbin： Harbin Institute of Technology，2020.

[4] VICTOR K， ALESSANDRO M， CIRO S， et al. Ascento： a Two-wheeled Jumping Robot［C］∥ International Conference on Robotics and Automation. Montreal， 2019： 7515-7521.

[5] YANG Z Y， BIAN Z K， ZHANG W， et al. Design and Control of Multi-mode Wheeled-bipedal Robot with Parallel Mechanism［C］∥ International Conference on Communications， Computing and Artificial Intelligence. Shanghai，2023： 69-74.

[6] YU J， ZHU Z， LU J， et al. Modeling and MPC-based Pose Tracking for Wheeled Bipedal Robot［J］. IEEE Robotics and Automation Letters， 2023， 8（12）： 7881-7888

[7] WANG S， CUI L L， ZHANG J F， et al. Balance Control of a Novel Wheel-legged Robot： Design and Experiments［C］ ∥International Conference on Robotics and Automation. Xi'an，2021：6782-6788.

[8] ZHANG J F， LI Z X Li， WANG S， et al. Adaptive Optimal Output Regulation for Wheel-legged Robot Ollie： a Data-driven Approach［J］. Frontiers in Neurorobotics.2023，16： 1102259.

[9] 单开正， 于海涛， 韩亮亮，等. 近似直驱双足机器人跳跃运动非线性优化及试验验证［J］， 机械工程学报， 2021，57（13）：153-162.

[10] SHAN Kaizheng， YU Haitao， HAN Liangliang， et al. Nonlinear Optimization and Experimental Validation of a Quasi-direct-drive Bipedal Robot’s Jumping Motion ［J］. Journal of Mechanical Engneering， 2021，57 （13）： 153-162.

[11] 耿雪晴， 吴孟武， 华林.轮腿式可变形车轮设计及整车控制研究［J］. 中国机械工程，2023， 34 （12）： 1446-1452.

[12] GENG Xueqing， WU Mengwu， HUA Lin. Study on Design of Wheel-leg Deformable Wheel and Vehicle Control［J］. China Mechanical Engineering，2023， 34 （12）： 1446-1452.

[13] BJELONIC M， BELLICOSO C D， DE V， et al. Keep Rollin’ Whole-body Motion Control and Planning for Wheeled Quadrupedal Robots［J］. IEEE Robotics and Automation Letters， 2019，4（2）：2116-2123.

[14] XIN Y X， CHAI H， LI Y B， et al. Speed and Acceleration Control for a Two Wheel-leg Robot Based on Distributed Dynamic Model and Whole-body Control［J］. IEEE Access，2019， （7）：180630-180639.

[15] 姜利彬. 一种双轮自平衡车的设计和实现［D］. 南京： 东南大学，2016.

[16] JIANG Libin. Design and Implementation of a Two-wheeled Self-balanced Vehicle［D］.Nanjing： Southeast University， 2016.

[17] ZHANG X F， ZHANG Y. H， SUN H. X，et al. Design and Dynamics Analysis of a Two-wheel Robot with Hopping Ability［J］. Robotics， 2014， 36： 355-361.

基金资助

湖南省自然科学基金(2022JJ30664)