Measurement Method of Expansion in the Expansion Joint of Heat Supply Network Based on Distributed Optical Fiber Sensing
-
摘要: 在热力管网中，膨胀节作为缓冲管道的关键部分，极易因变形扭曲引发泄漏故障，进而致使热网瘫痪.实时准确测量膨胀节的膨胀是评估管网健康状态的有效途径.对此，本文提出一种基于分布式光纤传感技术的膨胀节膨胀量的新型检测方法，建立了膨胀量检测模型，并通过实验验证了方法的有效性.研究结果表明，测量误差随着光纤绕制圈数增大而减小，通过调节绕制圈数，可以将误差范围控制在3%以内.这不仅为热网膨胀节的膨胀量检测提出了一种新思路，同时也为分布式光纤传感技术在热网健康状态检测领域的应用奠定了一定的理论基础.Abstract: As a key part of the buffer pipeline in the thermal network, the expansion joint can easily cause leakage failure due to its deformation and distortion, causing the thermal network to collapse. It is an important way to assess the health status of the pipeline network effectively by accurate measuring the expansion value of the expansion joint in real-time. Therefore, this paper presents a new detection method for expansion based on the distributed optical fiber sensing technology and establishes the expansion measurement model. The effectiveness of this method is verified by experiments. The research results show that the measurement error decreases as the number of optical fiber loops increases, and the measurement error can be controlled within 3% by adjusting the number of optical fiber loops. It not only provides a new idea for the expansion value detection of the expansion joint in heating network but also lays a theoretical basis for the application of distributed optical fiber sensing technology in the field of heating network.
-
Key words:
- Distributed /
- optical fiber sensing /
- heating network /
- expansion measurement
-
近年来, 轮式移动方式以其高效率、高能量利用率引起了腿足式机器人研究者的广泛关注[1]. 试图搭建腿轮构型机器人, 将腿式机器人的高越障能力与轮式运行方式的快速移动能力相结合. 因此, 双腿轮机器人、四腿轮机器人等平台应运而生[2]. 其中, 双腿轮机器人具有更加灵活的移动与转向性能, 在多种行业工况下具有广阔的应用前景[3].
为使双腿轮机器人的地形适应能力进一步提高, 面临较高垂直障碍时, 可采用跳跃形式快速跨越. 苏黎世理工大学研发的双腿轮机器人Accento[4-5]在基于线性二次调节器(Linear quadratic regulator, LQR)平衡控制器的基础上进行跳跃控制器的设计, 实现了跳跃运动, 收到了显著的效果. 美国波士顿动力公司研发的Handle双腿轮机器人[6]实现了快速加减速、跳跃、搬运重物等功能, 展现了良好的动态平衡能力. 如何构建一个可以将运动与平衡功能集于一体的具有扩展性的双腿轮控制框架, 成为一个亟待解决的问题. 但是, 双腿轮机器人系统为内在不稳定系统, 且具有欠驱动特性, 运动过程中不存在稳定平衡点, 因此需要借助复杂的控制算法使其保持动态平衡. 而针对该系统的高动态运动控制研究尚处于起步阶段.
移动机器人经过半个世纪的发展, 基于静态稳定的控制算法已成熟, 主要应用对象包括双足机器人[7]、四足机器人[8]、六足机器人[9]等, 该类算法在保证机器人静态稳定裕度的前提下进行位置控制, 实现期望运动, 其依据为ZMP (Zero moment point)[10]、ESM (Energy stability margin)[11]、NESM (Normalized ESM)[12]等静态稳定判据, 因此无法实现高动态运动控制. 而双腿轮机器人的支撑面为两轮与地面接触点的连线, 不存在静态稳定区域, 因此静态稳定算法对于双腿轮机器人的借鉴意义不大.
双轮机器人的控制方法已渐趋成熟, 作为一个欠驱动不稳定多耦合系统, 常用于控制算法的有效性验证[13-14], 日本庆应大学搭建了两轮移动机械臂平台, 验证了基于零空间的共振比率控制器[15]、滑膜控制器[16]、非线性反步法控制器等控制轮的移动与机械臂运动的有效性; 意大利技术研究院控制双足机器人站立于两轮移动平台Segway上[17], 采用二次优化方法以质心位置与角动量为调节目标, 生成全身关节力矩, 能够保持平衡并控制平台移动. 大连理工大学团队采用模型预测控制(Model predictive control, MPC)实现了两轮移动平台的运动轨迹跟踪[18]. 以上研究都具有借鉴意义, 但与两轮移动平台不同, 双腿轮平台不再存在两轮同轴被动约束, 这也使得控制器的构造难度大大增加.
近几年来, 腿足式机器人动态控制算法成为研究热点. 得益于各种实时优化解算库[19-20]的开发, 控制中的多解问题可转化为优化问题, 得到局部甚至全局最优解[21-22]. 如美国麻省理工学院的四足机器人Mini-cheetah[23]构造了全身控制与模型预测控制相结合的控制器, 实现了4 m/s的奔跑和碎石路面行走. 双足机器人Atlas[24]跟踪采用模型预测控制器优化的参考轨迹, 实现了左右脚交替三连跳40 cm台阶、前滚翻等高动态运动.
为提高双腿轮机器人的地形适应性, 实现稳定的速度跟随与跳跃, 本文在前期研究的基础上[25], 构造了基于最优力分配的全身力矩控制器并实现了跳跃运动, 其创新点如下:
1)实现了基于优化的自平衡全身力矩控制. 该方法可自行生成同时满足平衡与期望运动的全身力矩, 不需要设计平衡控制器. 受到跳跃着陆冲击干扰时, 能够有效规划期望状态轨迹, 并恢复有界稳定.
2)提出了适用于双腿轮机器人的连续跳跃规划. 可在保证高速运动的前提下, 实现垂直障碍的跨越, 并能够在腾空阶段对俯仰姿态进行有效调整, 保证着陆姿态.
1. 运动学与动力学建模
本文的控制对象以美国HEBI Robotics公司的双腿轮机器人HEBI IGOR[26]为原型, 其简化模型如图1所示. 该机器人具有一个单刚体躯干与两条结构相同相对于躯干左右对称布置的腿轮. 躯干尺寸为$200\;{\rm{ mm}}\times300\;{\rm{ mm}}\times200\; {\rm{mm}}\; (L\times w\times h)$, 质量为40 kg. 腿轮完全伸展长度为1150 mm, 其中大腿长度(L1)为500 mm, 小腿长度(L2)为500 mm, 轮的半径(R)为150 mm. 每条腿轮布置3 个旋转关节, 所有关节都可以实现独立的力矩控制. 髋部与膝部为俯仰旋转关节, 能够实现腿部的收缩与伸展, 轮通过一个俯仰旋转关节与腿部末端相连, 从而实现机器人在三维空间中的移动.
1.1 运动学建模
双腿轮机器人为典型的动基座系统, 浮动坐标系${\Sigma _B}$固连于躯干质心处, 其在惯性系中的线速度与角速度分别描述为${}^{\boldsymbol{O}}{{\boldsymbol{v}}_{\boldsymbol{B}}}$与${}^{\boldsymbol{O}}{{\boldsymbol{\omega}} _{\boldsymbol{B}}}$. 各肢体末端均由关节、连杆与躯干相连, 因此, 肢体末端广义速度同时受到躯干动基座广义速度与各关节角速度的影响.
$$\left[ {\begin{array}{*{20}{c}} {{}^{\boldsymbol{O}}{{\boldsymbol{v}}_{i}}} \\ {{}^{\boldsymbol{O}}{{\boldsymbol{\omega}} _{i}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\boldsymbol{E}}&{ - {}^{\boldsymbol{O}}{{\vec {\boldsymbol{r}}}_{\boldsymbol{B}i}}} \\ {\boldsymbol{0}}&{\boldsymbol{E}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{}^{\boldsymbol{O}}{{\boldsymbol{v}}_{\boldsymbol{B}}}} \\ {{}^{\boldsymbol{O}}{{\boldsymbol{\omega}} _{\boldsymbol{B}}}} \end{array}} \right] + {{\boldsymbol{J}}_{i}}{\dot {{\boldsymbol{q}}}_{i}}$$ (1) 其中, 本文中编号$i= 1, 2$分别代表右腿与左腿, ${}^{\boldsymbol{O}}{{\boldsymbol{v}}_{i}}$与${}^{\boldsymbol{O}}{{\boldsymbol{\omega}} _{i}}$分别为腿i末端相对于世界坐标系的速度与角速度. ${}^{\boldsymbol{O}}{\vec {\boldsymbol{r}}_{\boldsymbol{B}i}}$为由躯干坐标系到腿i末端坐标系的位矢在世界坐标系中的表示, ${{\boldsymbol{q}}_{i}}$为腿${i}$主动关节角度.
1.2 动力学建模
为便于构造分布式控制器, 采用了分布式动力学建模方法[25]. 分布式动力学模型由躯干子系统以及腿轮子系统组成. 腿轮子系统通过广义力旋量与躯干子系统形成迭代递推关系, 其广义力旋量的表示如图1所示. 躯干的动力学模型为
$${{\boldsymbol{M}}_{\boldsymbol{b}}}\left( {\boldsymbol{X}} \right)\ddot {\boldsymbol{X}}{\rm{\;+\;}}{{\boldsymbol{G}}_{\boldsymbol{b}}}\left( {\boldsymbol{X}} \right) = {{\boldsymbol{J}}^{{\rm{T}}}}{\boldsymbol{W}}$$ (2) 其中, ${\boldsymbol{X}} = {\left[ {{}^{O}{x_B}}\;\;{{}^{O}{y_B}} \;\; {{}^{O}{z_B}}\;\;\gamma \;\;\beta \;\;\alpha \right]^{\rm{T}}}$为躯干坐标系${\Sigma _B}$在大地坐标系${\Sigma _O}$中的位姿描述, ${{\boldsymbol{M}}_{\boldsymbol{b}}}$为惯性矩阵, ${{\boldsymbol{G}}_{\boldsymbol{b}}}$为重力补偿项. ${\boldsymbol{W}} = {\left[ {{\boldsymbol{W}}_{1}}\;\;{{{\boldsymbol{W}}_{2}}} \right]^{\rm{T}}}$为腿i末端施加于躯干的广义力旋量, 其中, ${{\boldsymbol{W}}_{i}} = {\left[{{f_{ix}}}\;\;{{f_{iz}}}\;\;{{n_{iy}}} \right]^{\rm{T}}}$. 支撑工况下, 腿轮施加于躯干的力旋量可作为躯干子系统的输入, 从而实现躯干子系统的控制. 由于轮式结构造成的非完整约束, 世界坐标系中躯干侧向运动(${}^{O}{\dot y_B}$)与前向运动(${}^{O}{\dot x_B}$)由躯干坐标系中x方向速度${}^B{{v}_{{B}}}$和航向角度($\alpha $)耦合而成, 基于非完整约束运动学构造矩阵${\boldsymbol{S}}$:
$${\boldsymbol{S}}\;{\rm{ = }}\left[ {\begin{array}{*{20}{c}} {\cos \alpha}&0&0&0&0 \\ {\sin \alpha }&0&0&0&0 \\ 0&1&0&0&0 \\ 0&0&1&0&0 \\ 0&0&0&1&0 \\ 0&0&0&0&1 \end{array}} \right]$$ (3) 使得${\dot{\boldsymbol X}}\;{\rm{=}}\;{\boldsymbol{S\eta}}$, 其中${\boldsymbol{\eta}}\; {\rm{=}}\;[{ {{}^B{{v}_{{B}}}}\;\;{{}^B{{\dot z}_B}}\;\;{\dot \gamma }\;\;{\dot \beta }\;\;{{\omega}}^{\rm{T}}}]$, ${{\omega}} $为航向角速度, 求导得$\ddot{\boldsymbol{X}}{\rm{ \;= \;}}{\boldsymbol{S}}\dot{\boldsymbol{\eta}} $, 因此, 式(2)中动力学模型可改写为
$${\boldsymbol{\Psi}} \dot {\boldsymbol{\eta}}\; {\rm{ + }}\;{{\boldsymbol{G}}_{\boldsymbol{b}}}\left( {\boldsymbol{X}} \right) = {{\boldsymbol{J}}^{{\rm{T}}}}{\boldsymbol{W}}$$ (4) 其中, ${\boldsymbol{\Psi}} ={{\boldsymbol{M}}_{\boldsymbol{b}}}{\boldsymbol{S}}$. 此时, 状态量${\boldsymbol{\eta}}$为躯干刚体在自然坐标系(始终与机体坐标系重合)中的运动描述.
与腿足式机器人不同, 双腿轮机器人处于支撑状态时, 腿轮不仅要提供躯干所需虚拟力, 还要在不同运动状态下保持唯一的平衡姿态. 因此, 以轮为动基座, 采用递推牛顿−欧拉方法建立支撑状态下的腿轮动力学方程如下所示:
$$\begin{split} &{{\boldsymbol{M}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\boldsymbol{q}}_{iw}}} \right){{\ddot {\boldsymbol{q}}}_{i}} + {{\boldsymbol{C}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\dot {\boldsymbol{q}}}_{i}},{{\boldsymbol{q}}_{iw}},{{\dot {\boldsymbol{q}}}_{iw}}} \right)+\\ & \quad{{\boldsymbol{G}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\boldsymbol{q}}_{iw}}} \right){\rm{ + }}\;{{\boldsymbol{I}}}\left( {{{q}_{iw}},{{\dot q}_{iw}}} \right){{\ddot q}_{iw}} + {\boldsymbol{J}}_{i}^{\rm{T}}{{\boldsymbol{W}}_{i}} = {{\boldsymbol{\tau}} _{i}} \end{split} $$ (5) 其中, ${{\boldsymbol{\tau}} _{i}} = {\left[ {{\tau _{iw}}}\;\;{{\tau _{ik}}}\;\;{{\tau _{ih}}} \right]^{\rm{T}}}$分别为腿i的轮关节、膝关节与髋关节力矩. ${{\boldsymbol{q}}_{i}}={\left[ {{{q}_{ic}}}\;\;{{{q}_{ik}}}\;\;{{{q}_{ih}}} \right]^{\rm{T}}}$分别为腿i 的小腿俯仰角度、膝关节与髋关节角度. ${{\boldsymbol{M}}_{i}}\left( {{{\boldsymbol{q}}_{i}}} \right)$、${{\boldsymbol{C}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\dot {\boldsymbol{q}}}_{i}}} \right)$、${{\boldsymbol{G}}_{i}}\left( {{{\boldsymbol{q}}_{i}}} \right)$分别为惯性矩阵、科氏力与离心力、重力补偿矩阵. 值得注意的是, 所提出的分布式动力学建模方法, 其关节角度变量中不包括轮式动基座的线运动与角运动, 但当双腿轮机器人进行高速运动时, 腿部连杆会产生较大的惯性力, 从而影响控制精确度与系统的平衡. 因此, ${\boldsymbol{I}}\left( {{{q}_{iw}},{{\dot q}_{iw}}} \right)$为动基座惯性力补偿矩阵, 以补偿高速运动时动基座传导给各腿轮的惯性力.
当双腿轮机器人处于腾空状态时, 将腿轮子系统分解为两个简化的模型 —— 腿部模型与轮式倒立摆模型, 腿部模型中髋关节坐标系与躯干坐标系方向相同, 以髋关节坐标系为基座, 建立动力学模型如下:
$${\boldsymbol{M}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s}} \right)\ddot {\boldsymbol{q}}_{i}^{s}\;+\; {\boldsymbol{C}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s},\dot {\boldsymbol{q}}_{i}^{s}} \right)\;+\;{\boldsymbol{G}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s}} \right) + {\boldsymbol{J}}_{i}^{{s}{\rm{T}}}{{\boldsymbol{F}}_{is}}{\rm{\;=\;}}{\boldsymbol{\tau}} _{i}^{s}$$ (6) 其中, ${\boldsymbol{\tau}} _{i}^{s} = \left[ {{\tau _{ih}}}\;\;{{\tau _{ik}}} \right]$, ${\boldsymbol{q}}_{i}^{s} = {\left[ {{{q}_{ih}}}\;\;{{{q}_{ik}}} \right]^{\rm{T}}}$.
为研究腾空阶段轮的转动对机器人姿态的影响, 将模型简化为一级轮式倒立摆, 即只关注矢状面内的运动, 将除轮以外的杆件质量集中于一个质点, 形成虚拟质心, 虚拟质心与轮轴的连线为虚拟倒立摆摆杆. 进行无外部接触力的拉格朗日动力学建模, 其动力学模型为
$$\begin{split} &\left( {{m_u} + {m_w} + \dfrac{{{I_w}}}{{{R^2}}}} \right)R{{\ddot \theta }_w} + L{m_u}\ddot \sigma {{\rm{c}} _\sigma } - L{m_u}{{\dot \sigma }^2}{{s} _\sigma } = 0 \\ &L{m_u}R{{{c}} _\sigma }{{\ddot \theta }_w} + \left( {{I_{b}} + {L^2}{m_u}} \right)\ddot \sigma - gL{m_u}{{s} _\sigma } = - {\tau _w} \\[-10pt] \end{split} $$ (7) 其中, ${m_u}$为除轮外其他杆件的质量总和, $\sigma $为倒立摆的虚拟俯仰角, L为虚拟摆杆长度, ${I_{b}}$与${I_w}$分别为摆杆与轮的转动惯量.
2. 轨迹生成器
构造轨迹生成器使机器人可根据障碍物的高度规划此次跳跃的躯干参考轨迹与足端参考轨迹. 本节中, 首先根据障碍物高度确定关键点处的位置与速度, 然后采用三次样条插值法得到躯干质心点与轮最小离地点的连续位置与速度. 简称轮最小离地点为足端. 相关参数的含义如表1所示.
表 1 跳跃参数设置Table 1 Parameters of the jump motion参数 参数含义 H0 正常行走时躯干站立高度在${\Sigma _O}$中的表示 H1 腾空瞬间躯干站立高度在${\Sigma _O}$中的表示 Hpt 足端最大离地距离在${\Sigma _O}$中的表示 Hs 足端最大收缩量在${\Sigma _B}$中的表示 图2展示了机器人跳跃时躯干与足端轨迹, 虚线表示躯干正常站立高度H0, 实线为跳跃过程中躯干的期望轨迹. 支撑相时, 机器人双轮着地; 腾空相时, 机器人双轮离地, 处于自由落体状态. 将跳跃动作分解为三阶段, 分别为起跳($p_0-p_1$)、腾空($p_1- p_3$)、缓冲($p_3-p_4$). 起跳阶段, 通过腿部快速伸展, 为躯干纵向加速, 当达到期望速度($p_1$)时, 腿部开始收缩, 进入腾空相. 腾空相躯干受到重力加速度, 纵向速度不断减小, 在此过程中, 腿部完成了收缩, 并伸展到可保持正常站高H0的长度, 准备落地. 缓冲阶段将纵向速度调整为0, 并保持期望站高.
图 2 躯干与足端矢状面轨迹示意图Fig. 2 The trajectory schematic of the torso and feet in the sagittal plane图2中, $ H_0 $、$ H_1 $以及腿部最大收缩量$H_s $ 为常量, 由机器人本体结构确定, $H_{pt} $由所要跨越的障碍高度确定. $p_2 $ 点的高度也随之确定: ${H_2}=H_s + {H_{pt}} $, 由于p2点的速度${{v}_2} = 0$, 根据能量守恒定理, 可得:
$$\left\{\begin{aligned} &{{v}_1} = \sqrt {2g\left( {{H_{pt}} + {H_s} - {H_1}} \right)} \\ &{{v}_{\rm{3}}} = \sqrt {2g\left( {{H_{pt}} + {H_s} - {H_{\rm{0}}}} \right)} \\ &{t_{01}} = \dfrac{{2\left( {{H_1} - {H_0}} \right)}}{{\sqrt {2g\left( {{H_{pt}} + {H_s} - {H_1}} \right)} }} \\ & {t_{12}} = \dfrac{{\sqrt {2g\left( {{H_{pt}} + {H_s} - {H_1}} \right)} }}{g} \\ & {t_{{\rm{23}}}} = \dfrac{{\sqrt {2g\left( {{H_{pt}} + {H_s} - {H_{\rm{0}}}} \right)} }}{g} \end{aligned}\right. $$ (8) 其中, ${{v}_1}$和${{v}_{\rm{3}}}$分别为点p1和p3处的速度. 足端离地时间即腾空时间为${t_{12}} + {t_{23}}$. 为同时保证每一段期望轨迹的平滑以及初始点与结束点的位置$\left( {{y_{{\rm{init}}}}},{{y_{{\rm{end}}}}}\right)$、速度$\left( {{{v}_{{\rm{init}}}}},{{{v}_{{\rm{end}}}}} \right)$要求, 采用三次样条函数生成连续轨迹. 起跳阶段的纵向位置期望轨迹、$p_1$-$p_2 $阶段、${p_2}$-${p_3}$ 阶段足端期望轨迹均由式(9)生成:
$$ \left\{\begin{aligned} &y=a{t}^{3}+b{t}^{2}+ct+d\\ &c={v}_{{\rm{init}}}\\ &d={y}_{{\rm{init}}}\\ &a=\dfrac{{v}_{{\rm{end}}}t-2{y}_{{\rm{end}}}+{v}_{{\rm{init}}}t+2{y}_{{\rm{init}}}}{T^{3}}\\ &b=\dfrac{{v}_{{\rm{end}}}-3a{T}^{2}-c}{2T}\end{aligned}\right.$$ (9) 其中, T为该段三次样条插值的总时长.
3. 全身力矩控制器
为实现机器人奔跑与跳跃的有效控制, 本节介绍了分布式控制器的设计方法. 首先基于躯干子系统的动力学模型, 采用二次规划 (Quadratic programming, QP)二次优化得到合理的虚拟作动力以使得躯干位姿满足控制要求. 采用逆动力学前馈补偿与逆运动学反馈补偿方法得到腿轮关节力矩.
3.1 基于最优虚拟力分配的躯干控制器
对于躯干单刚体进行静力分析, 假设躯干质量集中于几何中心处, 建立躯干质心合力与躯干受到的腿轮虚拟作动力的映射关系如下所示:
$$\left[ {\begin{array}{*{20}{c}} {\boldsymbol{F}} \\ {\boldsymbol{N}} \end{array}} \right]{\rm{\;+ \;}}{{\boldsymbol{G}}_{b}} = {\boldsymbol{A}}\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{W}}_{1}}} \\ {{{\boldsymbol{W}}_{2}}} \end{array}} \right]$$ (10) 其中, ${\boldsymbol{F}}{\rm{\;= [}}{F_x},{F_y},{F_z}{]^{\rm{T}}},\;{\boldsymbol{N}} = {[{N_x},{N_y},{N_z}]^{\rm{T}}}$分别为躯干质心合力与力矩. 受到关节配置的约束, 腿轮末端只能主动输出延机体坐标系x和z的力与绕y轴的力矩, 式(10)可展开为式(11)的形式.
$$\begin{split} \left[ {\begin{array}{*{20}{c}} {\boldsymbol{F}} \\ {\boldsymbol{N}} \end{array}} \right] + &\left[ {\begin{array}{*{20}{c}} {{m_{{b}}}g\sin \beta } \\ 0 \\ {{m_{{b}}}g\cos \beta } \\ {\rm{0}} \\ {\rm{0}} \\ {\rm{0}} \end{array}} \right] = \\ &\underbrace {\left[ {\begin{array}{*{20}{c}} 1&0&0&1&0&0 \\ 0&0&0&0&0&0 \\ 0&1&0&0&1&0 \\ 0&{\dfrac{w}{2}}&0&0&{ - \dfrac{w}{2}}&0 \\ { - \dfrac{h}{2}}&0&1&{ - \dfrac{h}{2}}&0&1 \\ { - \dfrac{w}{2}}&0&0&{\dfrac{w}{2}}&0&0 \end{array}} \right]}_{\boldsymbol{A}}\left[ {\begin{array}{*{20}{c}} {{f_{1x}}} \\ {{f_{1z}}} \\ {{n_{1y}}} \\ {{f_{2x}}} \\ {{f_{2z}}} \\ {{n_{2y}}} \end{array}} \right]\end{split}$$ (11) 其中, $g = 9.8 \; {\rm{m}}/{{\rm{s}}^2}$. 根据不同维度的特点构造躯干期望虚拟力${\boldsymbol{F}}$与${\boldsymbol{N}}$的反馈部分, 基于动力学模型进行必要的前馈补偿, 为保证速度跟踪性能, 虚拟力${F_x}$的反馈部分采用自适应滑膜方法构造, 其他维度采用比例−微分控制方法构造, 具体方法为
$$\left\{\begin{aligned} &{F_x} = s + sat\left( s \right)c \\ &s = \dot e + \lambda e \\ &e = {{\dot x}_d} - {}^B{{{v}}_{{B}}}, \dot e = {{\ddot x}_d} - {}^B{{\dot v}_B} \\ &{F_z} = {k_{pz}}\left( {{z_d} - {}^B{z_B}} \right) + {k_{dz}}\left( {{{\dot z}_d} - {}^B{{\dot z}_B}} \right)+{m_{{b}}}{{\ddot z}_d} \\ &{N_x} = {k_{p\gamma }}\left( {{\gamma _d} - \gamma } \right) + {k_{n\gamma }}\left( {{{\dot \gamma }_d} - \dot \gamma } \right) \\ & {N_y} = {k_{p\beta }}\left( {{\beta _d} - \beta } \right) + {k_{n\beta }}\left( {{{\dot \beta }_d} - \dot \beta } \right) \\ &{N_z} = {k_{{\omega}} }\left( {{{{\omega}} _d} - {{\omega}} } \right) \end{aligned}\right. $$ (12) 矩阵${\boldsymbol{A}}$为奇异矩阵, 因此无法直接解得与躯干虚拟广义力一一对应的腿轮末端作动力, 因此, 将式(11)变形为
$${\boldsymbol{A}}\underbrace {\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{W}}_{1}}} \\ {{{\boldsymbol{W}}_{2}}} \end{array}} \right]}_x=\underbrace {{{\left[ {\begin{array}{*{20}{c}} {\boldsymbol{F}} \\ {\boldsymbol{N}} \end{array}} \right]}^{{\rm{desire}}}}+{{\boldsymbol{G}}_{b}}}_b$$ (13) 通过QP优化器进行作动力求解
$$\begin{split} &\mathop {\min }\limits_{\boldsymbol{x}} \;\;\;f =\; {\left( {{\boldsymbol{Ax}} - {\boldsymbol{b}}} \right)^{\rm{T}}}{\boldsymbol{Q}}\left( {{\boldsymbol{Ax}} - {\boldsymbol{b}}} \right)\;+ \\ & \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{\boldsymbol{P}}{\left\| {{\boldsymbol{x}} - {{\boldsymbol{x}}_{{\rm{pre}}}}} \right\|^2} + {\boldsymbol{J}}{\left\| {\boldsymbol{x}} \right\|^2} \\ &{\rm{s.t}}\; \left| {{f_{ix}}} \right| \leq \mu {f_{iz}} \end{split} $$ (14) 其中, Q, P, J为正定对角权重矩阵, ${{\boldsymbol{x}}_{{\rm{pre}}}}$为上一时刻的解.
3.2 腿部控制器
当机器人处于支撑相时, 躯干控制器将躯干期望合力分配为各条腿轮的末端力旋量. 为实现力伺服, 基于轮式基座动力学模型前馈补偿的逆动力学力矩解算器如下所示
$$\begin{split} {{\boldsymbol{\tau}} _{i}} =\;& {\boldsymbol{J}}_{i}^{\rm{T}}{{\boldsymbol{W}}_{i}} + {{\boldsymbol{C}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\dot {\boldsymbol{q}}}_{i}},{{q}_{iw}},{{\dot q}_{iw}}} \right) + \\ &{{\boldsymbol{G}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{q}_{iw}}} \right) +{i}\left( {{{q}_{iw}},{{\dot q}_{iw}}} \right){{\ddot q}_{iw}} \end{split} $$ (15) 其中, ${q}_{iw}{\text{, }}{\dot{q}}_{iw}{\text{, }}{\ddot{q}}_{iw}$均由基于传感器的状态观测器得到. 基于动力学前馈补偿的控制方法可以使机器人自适应地保持在唯一的不稳定平衡点处.
当机器人处于腾空相时, 为保证腿轮末端与躯干的相对位置以及机器人在世界坐标系中姿态的稳定, 将腾空相腿部控制器分为两部分, 基于式(6)动力学模型, 对髋关节与膝关节进行力矩控制以实现腿轮末端轨迹跟踪.
$${\boldsymbol{\tau}} _{i}^{s} = {\boldsymbol{C}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s},\dot {\boldsymbol{q}}_{i}^{s}} \right) + {\boldsymbol{G}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s}} \right) + {\boldsymbol{J}}_{i}^{{s}{\rm{T}}}{{\boldsymbol{F}}_{is}}$$ (16) 其中, ${{\boldsymbol{F}}_{is}} = {\left[ {{f_{ipx}}}\;\;{{f_{ipz}}}\right]^{\rm{T}}}$.
$$\left\{\begin{aligned} &{f_{ipx}} = {k_{ipx}}\left( {{x_{pd}} - {x_{ip}}} \right) + {k_{idx}}\left( {{{\dot x}_{pd}} - {{\dot x}_{ip}}} \right) \\ &{f_{ipz}} = {k_{ipz}}\left( {{z_{pd}} - {z_{ip}}} \right) + {k_{idz}}\left( {{{\dot z}_{pd}} - {{\dot z}_{ip}}} \right) \end{aligned}\right. $$ (17) 其中, $\left( {{x_{ip}},{z_{ip}}} \right)$与$\left( {{x_{pd}},{z_{pd}}} \right)$分别为自然坐标系中的足端位置描述与期望足端位置描述.
在腾空阶段, 系统处于动量守恒状态, 因此腿部关节的运动会导致整体姿态的变化, 此时可通过控制飞轮转动对姿态的误差进行有效补偿. 将一级轮式倒立摆动力学模型式(7)变形, 得到:
$$\ddot \beta = - \frac{{L{m_{b}}R{{\rm{c}} _\beta }}}{{{I_{b}} + {L^2}{m_{b}}}}{\ddot \theta _w} - \frac{1}{{{I_{b}} + {L^2}{m_{b}}}}{\tau _w} + \frac{{gL{m_{b}}{{\rm{s}} _\beta }}}{{{I_{b}} + {L^2}{m_{b}}}}$$ (18) 此时轮不与地面接触, 可以看作调整平衡的飞轮, 通过式(18)可以看出, 飞轮的转动能够影响倒立摆的俯仰角运动状态, 当俯仰角逆时针偏离期望位置时, 飞轮应顺时针转动保证姿态平衡, 反之亦然. 基于这一结论, 可以根据躯干控制器中输出的俯仰姿态矫正力矩${N_y}$ (如式(12)所示)来规划轮的力矩, 从而得到轮关节的力矩为
$$ {\tau _{{\rm{1}}w}} = {\tau _{2w}} = - k{N_y} $$ (19) 其中, k为正比例增益.
4. 仿真实验
在Webots 8.4.0中建立与HEBI IGOR同尺寸3D模型, 与Visual Studio 2017、Gurobi 902优化解算库进行联合仿真. 为验证本文所提算法的有效性, 分别设置了连续跳跃实验与空中姿态调整对比实验, 其总体控制框图如图3所示. 仿真视频见
https://www.bilibili.com/video/BV1zz4y1X7jp/ .根据期望跳跃高度${H_{pt}}$的不同, 轨迹生成器会生成不同的起跳阶段躯干纵向期望轨迹. 分别针对${H_{pt}}\;{ = 0}.45\;{\rm{m}}$, ${H_{pt}}\;{ = 0}.35\;{\rm{m}}$, ${H_{pt}}\;{ = 0}.25\;{\rm{m}}$进行了弹跳实验, 机器人的躯干纵向轨迹${z_{b}}$以及足端位置轨迹${z_p}$如图4所示, 可以看出期望弹跳高度越大, 所规划的躯干纵向期望轨迹在起跳点处纵向速度越大, 从而腾空相时间越长.
图 4 不同离地高度下躯干与足端纵向位置曲线Fig. 4 Longitudinal position curves of the torso and feet at different ground clearance4.1 连续跳跃实验
本仿真实验主要验证: 1)该算法框架下的跳跃动作是否能够实现; 2)该算法框架是否能够对跳跃动作带来的强干扰进行抑制并保持有界稳定. 因此, 仿真实验设置如下: 机器人初始状态为站立静止, 在0 ~ 3 s内加速至2 m/s, 随后保持匀速运动. 当t = 3.35 s时, 通过外部遥控下达${{H}_{{pt}}}{\rm{\;= \;0}}.45\;{\rm{m}}$的跳跃命令, 机器人依次执行起跳−腾空−落地缓冲−调整; 当t = 5.85 s时, 再次下达${H_{pt}}=0.45\;{\rm{m}}$的跳跃命令, 机器人完成跳跃并最终将跳跃导致的状态偏移调整到有界稳定状态. 以首次跳跃为例, 其仿真截图如图5所示.
对于完全站立高度为1.35 m的腿轮机器人来说, 最大离地高度为0.45 m的跳跃会对其平衡状态产生较大冲击, 甚至失去平衡. 由图6和图7中曲线可看出, 每次跳跃结束后, 各状态量进行了及时调整. 0 ~ 3 s加速阶段躯干水平速度的最大跟踪误差不超过0.075 rad, 其他支撑阶段, 误差小于0.04 rad. 图7可见, 处于支撑相时, 加速度越大水平速度误差越大, 最大误差为0.06 m/s. 当机器人进入腾空相时, 与外界环境不再产生相互作用力, 此时, 机器人保持动量守恒状态, 因此腿部伸缩运动影响了躯干的水平速度, 最大误差不超过0.08 m/s. 由腾空相切换为支撑相的瞬间, 实际水平速度与期望速度(2 m/s)之间存在偏差, 控制器实时规划期望速度曲线, 并将速度调整为2 m/s. 此外, 如图6所示, 腾空相中机器人pitch与roll角波动较大, 但着陆后控制器可有效控制姿态角恢复到0 rad附近. 与之对应的, 如图8所示, 力与力矩在着陆后出现较明显波动, 用以调整状态量使其恢复临界稳定.
图8为躯干子系统所需的虚拟广义力曲线图, 可见, 前进方向虚拟作动力${F_x}$与躯干水平加速度相关, 0 ~ 3 s水平加速度始终为正, 因此, ${{F_x}> 0}$, 最大值约为46 N; 当站高保持常值时, 纵向虚拟作动力${F_z}$在388 N ~ 398 N (mb = 40 kg)区间内波动, 由于起跳瞬间躯干纵向期望轨迹的初始点处加速度不为零, 加速度跳变导致了${F_z}$的跳变.
躯干纵向位置跟踪曲线如图9所示, 最大误差为48 mm, 发生在起跳阶段, 当期望站高为常量${H_{\rm{0}}}$时, 最大误差为0.2 mm, 发生在落地缓冲阶段. 可见该控制框架具有对纵向位置跟踪的稳定性与抗扰能力. 腾空阶段, 足端位置基于躯干动基座计算得到, 从而躯干动基座的波动会影响足端轨迹跟随, 导致足端轨迹波动较大, 最大误差为50 mm.
通过最优力分配将躯干虚拟作动力分配为各腿轮末端期望的输出广义力如图10所示. 两腿轮的广义力输出基本保持一致, 在着陆后有明显的调整行为. 各关节力矩曲线如图11所示, 膝关节力矩输出较大, 髋关节与轮关节力矩均在0附近波动.
4.2 空中姿态调整对比实验
为验证轮轴力矩对俯仰姿态矫正的有效性, 设计了腾空姿态调整对比实验. 实验1中腾空相的轮轴力矩${\tau _w} = 0\;({\rm{N}}\cdot{\rm{m}})$, 实验2中腾空相的轮轴力矩${\tau _w} = - {N_y}$, 保证其他条件完全相同. 图12为对比实验的俯仰角曲线, 可见当${\tau _w} = 0\;({\rm{N}}\cdot{\rm{m}})$时, 俯仰角由腾空初始时刻到着陆瞬间, 一直处于单向递增状态, 若腾空时间过长会导致着陆姿态不稳定. 当${\tau _w} =- {N_y} $时, 以俯仰角状态量为反馈进行了有效的姿态调整, 使得腾空相俯仰角在 $(- 0.015\;{\rm {rad}} $, 0.02 rad)范围内波动. 证明了基于角动量守恒的轮的反向转动对腾空姿态的调整是有效的.
5. 结束语
为使双腿轮机器人实现高动态运动, 本文提出了基于最优力分配的全身力矩控制器, 该控制器基于动力学模型进行设计, 所生成的全身力矩可在不额外规划平衡控制器的基础上实现机器人的自平衡, 并有效控制躯干位姿. 仿真实验表明, 该控制器能够同时保证水平速度、站高等多个维度的状态量处于有界稳定. 为提高双腿轮机器人地形适应能力, 规划并实现了双腿轮机器人的跳跃运动, 使其能在不打断高速运动的同时跨越垂直障碍, 进行了连续跳跃实验, 验证了所提方法的有效性.
-
图 2 膨胀节半剖图
1波纹管, 2、3接管, 4垫环, 5内衬筒(导流筒), 6耳板, 7双头螺柱, 8螺母
Fig. 2 Semi caesarean section of expansion joint
图 3 测试系统示意图
1分布式光纤传感系统, 2光纤线缆, 3柱形绝热体, 4导热体, 5端板, 6管道, 7双头螺柱, 8螺母, 9小波纹管, 10膨胀节环板, 11大波纹管, 12钢筋, 13光纤线缆切面, 14支撑架, 15地面, 16右侧管道, 17光纤的尾端
Fig. 3 The diagram of test system
图 6 不同载体膨胀量测量相对误差分布图
Fig. 6 Distribution diagram of the expansion relative error in different carriers
表 1 实验器材表
Table 1 Experimental equipment table
名称 型号 直径(mm) 长度(mm) 温度范围(℃) 光纤线缆 GJFJKZH-1A1B 3.3 62 800 $-40\sim 120$ 柱形绝缘体 PVC 250 600 $\setminus$ 抽水泵 RS-2688 2 700 $\setminus$ 恒温水浴箱 HH600 $\setminus$ $\setminus$ $0\sim100$ 
下载: 导出CSV
表 2 实验数据表
Table 2 Experimental data table
圈数 光纤线缆长度(mm) 显示距离(mm) 实际膨胀量(mm) 计算膨胀量(mm) 相对误差 0 0 0 0 0 0 1 785 1 000 3.3 4.2 0.27 5 3 925 4 000 16.5 16.82 0.0194 10 7 850 8 000 33.0 33.63 0.0191 15 11 775 12 000 49.5 50.45 0.0192 20 15 700 16 000 66.0 67.26 0.0191 25 19 625 20 000 82.5 84.08 0.0192 30 23 550 24 000 99.0 96.69 0.0233 35 27 475 27 000 115.5 113.50 0.0173 40 31 400 31 000 132.0 130.32 0.0127 45 35 325 35 000 148.5 147.13 0.0092 50 39 250 39 000 165.0 163.95 0.0064 55 43 175 43 000 181.5 180.76 0.0041 60 47 100 47 000 198.0 197.57 0.0022 65 51 025 51 000 214.5 214.39 0.0005 70 54 950 55 000 231.0 231.2 0.0008 75 58 875 59 000 247.5 248.02 0.0021
下载: 导出CSV
-
[1] 李艳珍, 邓志丹.波纹管补偿器在供热管网工程设计中的应用.化工设计通讯, 2017, 43(02):139-140 doi: 10.3969/j.issn.1003-6490.2017.02.112Li Yan-Zhen, Deng Zhi-Dan. Application of bellows compensator in engineering hesign of heating pipe network. Chemical Engineering Design Communications, 2017, 43(02):139-140 doi: 10.3969/j.issn.1003-6490.2017.02.112 [2] Chugunov M V, Polunina I N. Optimum design of the metal bellows on the SolidWorks platform. Mordovia University Bulletin, 2017, 27(2):169-177 doi: 10.15507/0236-2910.027.201702.169-177 [3] Zhang X, Ma Y, Zou H Y, Tao S, Zhang Q C, Zhang E Q. Experimental study:correcting the calculation formula for a welded metal bellows' effective diameter. Sealing Technology, 2016, 2016(10):7-12 doi: 10.1016/S1350-4789(16)30326-9 [4] Snedden N W. Analysis and design guidance for the lateral stiffness of bellows expansion joints. Thin-Walled Structures, 1985, 3(2):145-162 doi: 10.1016/0263-8231(85)90030-8 [5] Park D J, Yu J M, Han S Y, Yoon K B, Ji Y K. A study on system stress analysis of high temperature plant piping with expansion joints and load hangers. Journal of Energy Engineering, 2014, 23(3):116-124 doi: 10.5855/ENERGY.2014.23.3.116 [6] 崔亚飞, 刘英, 朱庆南, 王一宁, 杨雨图, 缑斌丽.波纹管膨胀节波形曲线拟合及波距检测算法研究.北京化工大学学报(自然科学版), 2018, 45(01):84-90 http://d.old.wanfangdata.com.cn/Periodical/bjhgdxxb201801014Cui Ya-Fei, Liu Ying, Zhu Qing-Nan, Wang Yi-Ning, Yang Yu-Tu, Gou Bin-Li. Research on the algorithm for fitting wave curve and calculating wave distance of bellows expansion joints. Journal of Beijing University of Chemical Technology, 2018, 45(01):84-90 http://d.old.wanfangdata.com.cn/Periodical/bjhgdxxb201801014 [7] 王晓嘉, 高隽, 王磊.激光三角法综述.仪器仪表学报, 2004, s2(8):601-604, 60 http://d.old.wanfangdata.com.cn/Periodical/yqyb2004z3185Wang Xiao-Jia, Gao Jun, Wang Lei. Survey on the laser triangulation. Chinese Journal of Scientific Instrument, 2004, s2(8):601-604, 60 http://d.old.wanfangdata.com.cn/Periodical/yqyb2004z3185 [8] 陈喜锋, 王国利, 郭雪梅.实现目标跟踪的光纤传感网络及其优化设计.自动化学报, 2014, 40(7):1348-1354 http://www.aas.net.cn/CN/abstract/abstract18406.shtmlChen Xi-Feng, Wang Guo-Li, Guo Xue-Mei. A fiber sensor network and its optimization design for target tracking. Acta Automatica Sinica, 2014, 40(7):1348-1354 http://www.aas.net.cn/CN/abstract/abstract18406.shtml [9] 刘铁根, 于哲, 江俊峰, 等.分立式与分布式光纤传感关键技术研究进展.物理学报, 2017, 66(07):60-76 http://d.old.wanfangdata.com.cn/Periodical/wlxb201709041Liu Tie-Gen, Yu Zhe, Jiang Jun-Feng, et al. Advances of some critical technologies in discrete and distributed optical fiber sensing research. Acta Physica Sinica, 2017, 66(07):60-76 http://d.old.wanfangdata.com.cn/Periodical/wlxb201709041 [10] Kuznetsov A G, Kharenko D S, Babin S A, Tsydenzhapov I B, Shelemba I S. Ultralong fibre-optic distributed Raman temperature sensor. Quantum Electronics, 2017, 47(10):976-970 http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=IOP_9375309 [11] Saxena M K, Raju S D V S J, Arya R, et al. Raman optical fiber distributed temperature sensor using wavelet transform based simplified signal processing of Raman backscattered signals. Optics & Laser Technology, 2015, 65(1):14-24 http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=ddc2b70c9094496bf7d7d7c048ff2376 [12] 张明江, 李健, 刘毅, 等.面向分布式光纤拉曼测温的新型温度解调方法.中国激光, 2017, 44(03):219-226 http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=zgjg201703030Zhang Ming-Jiang, Li Jian, Liu Yi, et al. Temperature demodulation method for distributed fiber raman temperature measurement. Chinese Journal of Lasers, 2017, 44(03):219-226 http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=zgjg201703030 [13] Cangialosi C, Ouerdane Y, Girard S, et al. Development of a temperature distributed monitoring system based on Raman scattering in harsh environment. IEEE Transactions on Nuclear Science, 2014, 61(6):3315-3322 doi: 10.1109/TNS.2014.2368787 [14] Kashyap R, Loranger S, Iezzi V L. High sensitivity distributed temperature fiber sensor using stimulated Brillouin scattering. Optics Express, 2017, 25(26):32591 doi: 10.1364/OE.25.032591 [15] Ding Z, Yang D, Liu K, et al. Long-range OFDR-based distributed vibration optical fiber sensor by multicharacteristics of rayleigh scattering. Procedia Engineering, 2017, 9(5):1-10 [16] Amira Z, Mohamed B, Tahar E. Monitoring of temperature in distributed optical sensor:Raman and Brillouin spectrum. Optik-International Journal for Light and Electron Optics, 2016, 127(8):4162-4166 doi: 10.1016/j.ijleo.2016.01.073 [17] 佟敬阔, 靳宝全, 王东, 王宇, 余辉, 白亮.面向管道安全监测的R-OTDR分布式光纤测温系统.传感技术学报, 2018, 31(01):158-162 doi: 10.3969/j.issn.1004-1699.2018.01.026Tong Jing-Kuo, Jin Bao-Quan, Wang Dong, Wang Yu, Yu Hui, Bai Liang. Distributed optical fiber temperature measurement system for pipeline safety monitoring based on R-OTDR. Chinese Journal of Sensors & Actuators, 2018, 31(01):158-162 doi: 10.3969/j.issn.1004-1699.2018.01.026 [18] 杨汉瑞, 李勇勇, 闫爱军, 孙佰仲, 葛粤, 纪克亮, 等.一种基于分布式光纤温度传感系统的供热管道膨胀节的状态监测装置及其方法.中国专利: CN108896203A, 2018年11月27日Yang Han-Rui, Li Yong-Yong, Yan Ai-Jun, Sun Bai-Zhong, Ge-Yue, Ji Ke-Liang, et al. The Monitoring Device And Method for the Expansion Joint of Heating Pipeline Based on Distributed Optical Fiber Temperature Sensing System. China Patent CN108896203A, issued November 27, 2018 期刊类型引用(5)
1. 周守军，张国正，刘臣，林旭东，曹长红. 基于CUSUM方法的集中供热管网泄漏故障诊断. 山东建筑大学学报. 2023(05): 48-57 . 
百度学术2. 解琳，佟璐. 大数据分析技术的光纤传感谱形复用技术研究. 激光杂志. 2021(01): 149-153 . 百度学术3. 吴自强，范伟松，斯荣. 应用单芯单模光纤的电缆沟多参量分布式光纤传感系统. 电子设计工程. 2021(04): 29-32+37 . 百度学术4. 白旭峰. 基于分布式光纤的网络化远程控制系统. 激光杂志. 2021(03): 110-114 . 百度学术5. 杨小琴，李凌. 基于嵌入式技术的激光传感器网络加密策略. 激光杂志. 2020(11): 120-124 . 百度学术其他类型引用(5)
-
下载:
计量
- 文章访问数: 1971
- HTML全文浏览量: 402
- PDF下载量: 102
- 被引次数: 10
