基于线性二次型调节器与比例积分微分控制的步态轨迹跟踪研究与优化

Research and Optimization of Gait Trajectory Tracking Based on Linear Quadratic Regulator and Proportional-Integral-Derivative Control

摘要: 老龄化或疾病导致下肢功能障碍的患者逐渐增多，传统的康复方式存在一定局限性，因此下肢康复机器人成为当前康复技术主流领域。为了在下肢康复设备轨迹追踪性能良好的同时保证患者使用时的安全性，本文设计了一种将线性二次型调节器(LQR)与比例积分微分(PID)控制相结合的控制算法。首先利用运动捕捉设备采集正常步态轨迹，然后在已搭建的足底驱动式踏板步态训练设备中，对步进伺服控制系统采用位置环–速度环–电流环三闭环的PID控制。求得系统数学模型后，使用LQR算法优化PID控制器各环反馈系数，然后将控制器部署至装置中，并进行了实验验证。实验验证了控制器的可行性与有效性，提升了康复设备追踪步态轨迹的性能。

Abstract: The number of patients with lower limb dysfunction caused by aging or disease is gradually in-creasing. Traditional rehabilitation methods have limitations. Therefore, lower limb rehabilitation robots have become the mainstream field of current rehabilitation technology. In order to ensure the safety of patients while ensuring the good trajectory tracking performance of lower limb reha-bilitation equipment, a control algorithm combining linear-quadratic-regulator (LQR) and propor-tional-integral-differential (PID) control is designed in this paper. Firstly, the normal gait trajectory is collected by using the motion capture device. Then, in the established foot-driven pedal gait training equipment, the step servo control system adopts the three closed-loop PID controls of posi-tion loop, speed loop and current loop. After obtaining the mathematical model of the system, the LQR algorithm is used to optimize the feedback coefficients of each loop of the PID controller, and then the controller is deployed to the device, and the experimental verification is carried out. The experiment verifies the feasibility and effectiveness of the controller and improves the performance of the rehabilitation equipment to track the gait trajectory.

1. 引言

根据第七次全国人口普查结果表明，我国60岁以上的老年人占比18.7%，我国发展进入了老龄化社会 [1] 。因老龄化等原因造成下肢运动功能障碍的人数逐年增多，对患者的日常出行、生活和工作能力产生巨大的影响。临床证明，除了早期的手术和药物治疗以外，使用器械辅助进行运动康复训练皆有助于患者康复 [2] [3] 。下肢康复机器人是能够有效地改善和恢复患者的下肢运动能力的一种康复器械 [4] 。目前下肢康复器械的驱动方式主要分为足底驱动式与下肢外骨骼式。外骨骼式下肢机器人能有效地训练患者下肢，但其结构会使关节运动受限；足底驱动式康复设备采用踏板带动患者足部进行训练，关节受限小。在下肢康复训练中，康复设备能否精准地按照设定的轨迹和速度运行，直接影响康复的效果和安全性 [5] 。目前下肢康复机器人电机运动控制有比例积分微分(proportional-integral derivative, PID)算法、模糊算法、神经网络控制算法、线性二次型调节(linear quadratic regulator, LQR)算法、自适应控制算法、滑模控制算法和模型预测控制(model predictive control, MPC)算法等，按不同的控制量可分为力矩控制、位置控制和速度控制 [6] - [11] 。在一些开环系统中，由于电机在运行过程中会产生振荡以及系统结构引起的位置和速度误差等问题，电机控制系统大多采用速度环和位置环双环或是加入电流环形成三闭环的PID控制方式 [12] [13] 。而各环之间相互影响导致参数的调整存在难度。LQR算法可对控制信号进行约束，能保证跟踪性能和控制量皆为最优，同时LQR格式相对固定，只需得到系统数学模型就能得到较好参数，降低了调参的难度。因此本文拟对足底驱动式下肢康复训练设备中的踏板步态训练装置进行研究，利用PID算法对各电机进行闭环控制，并在所设计的三环PID控制步进伺服驱动系统的基础上，加入LQR算法，优化设备步态轨迹的跟踪性能，实现步态轨迹精准的下肢运动康复训练。

2. 步态轨迹规划策略

2.1. 步态轨迹规划

行走作为人类日常基本活动之一，涉及到肌肉、神经和骨骼等多种因素，而下肢功能障碍患者往往因肌肉或是神经受损，行走过程失去协调，无法以正常步态行走，因此需要外力辅助或者通过康复设备训练恢复实现完整步态周期行走 [14] 。步态周期是指行走时同侧足跟第一次着地到第二次着地的过程，以秒为单位。人体完整的步态周期可分为支撑相与摆动相，可分为八个时相，其中单侧下肢步态周期的各分期状态如图1所示 [15] 。八个步态时相形成一个完整步态周期，一个周期内的连续运动构成运动轨迹。本文采用Polhemus Liberty电磁位置跟踪系统，该系统由传感器接收磁场发射源传出的信号，可以追踪物体的空间运动轨迹。将传感器安装在受试者踝关节处，在平坦地面进行一个步态周期的平地行走，采集踝关节轨迹坐标数据。再参考德国柏林技术大学踏板式步态训练器Haptic模拟平地走路的运动跟踪轨迹模型 [16] ，在数据分析软件MATLAB (R2021a，MathWorks Inc., USA)中绘制符合该模型的单个周期步态踝关节的轨迹并输出为XY坐标点，如图1所示。

Figure 1. Plan of gait trajectory

图1. 步态轨迹规划

2.2. 步态训练装置设计

传统的悬挂式机器人以悬挂绳减少患者下肢承重，保证患者训练的安全性。末端驱动踏板式的悬挂机器人相比跑步机式机器人单一的训练轨迹，能以踏板带动患者足底进行平地行走和上下楼梯等不同轨迹的康复训练。但由于减重装置的安装位置，长时间的训练会增加患者上半身的负担造成疲劳，且悬挂式机器人体积较大，训练场所有所限制。而外骨骼式机器人则因其轻便性以及灵活性，能够适应不同的场景，对患者进行不同步态轨迹的训练，成为了现有下肢康复机器人的主要研究领域。但外骨骼式机器人对于控制性能上要求较高，导致成本升高，且对各关节约束较大，患者一般无法独立使用，需要看护人员陪同。现有的悬挂式机器人融合外骨骼式机器人以及虚拟现实技术 [17] [18] [19] ，在降低设备体积的同时，增加了训练模式，但同时会使得成本上升，无法普及。

本文设计的下肢康复系统在移动平台的基础上，结合减重模块和末端踏板机构。减重系统采用悬吊减重和支撑减重，能保证患者的安全和减少下肢负载，减小了设备体积。末端踏板机构以足底驱动的方式带动患者下肢完成训练，对患者关节的约束较小，穿戴方便。装置能以不同轨迹运行，丰富训练模式。移动平台采用四轮结构，由后轮差速驱动，控制前进与转向，能够满足不同训练场景的需求。踏板步态训练装置如图2所示，图中部件1为与丝杆连接的丝杆电机，其与部件5滑块连接，带动踏板水平移动，部件2为与部件4齿条相啮合的齿轮电机，齿条与滑块相连带动踏板垂直方向上的位移，部件3中踝关节电机带动踏板旋转，三个电机控制患者下肢完成连续步态轨迹的训练；设计采用的驱动电机均为二相步进电机，步进电机因其成本低、定位精度高以及无累计误差等优点被广泛应用在数字控制系统中 [20] 。其接收脉冲输入信号并转化为轴转动，由脉冲个数和频率控制转动角度和转动速度，在转速和负载较高的系统中，会产生丢步和越步现象，通常结合闭环控制技术减少此现象 [21] 。

Figure 2. Lower limbs training device

图2. 下肢康复训练装置

3. 步态轨迹控制器设计

为提高系统运行的精度，减少步态训练装置运行过程中产生的误差，使用闭环反馈控制方式，采用编码器作为反馈单元，结合控制算法设计系统控制器。如图3所示，步进伺服驱动系统是数字步进驱动器技术结合伺服控制技术，采用电流环、速度环和位置环三环控制方法，满足装置低噪音运行、响应速度快和高负载平稳运行的要求，使得系统的控制更加稳定、更加精确。

Figure 3. Lower limbs training device

图3. 步进伺服驱动系统

旋转坐标系下的二相步进电机电压方程如式(1)所示，将定子电流对磁链的影响、磁滞和涡流效应以及二相绕组之间的互感等影响忽略，只考虑气隙磁导的平均分量和基波分量，可将电压方程简化为如式 (2) 所示 [22] 。

$\{\begin{array}{l}{U}_A=R{i}_A+\left(L_0-L_1\cos 2\theta \right)\frac{\text{d}{i}_A}{\text{d}t}-L_1\sin 2\theta \frac{\text{d}{i}_B}{\text{d}t}+2L_1\left(i_A\sin 2\theta -i_B\cos 2\theta \right)-k_e{\omega }_r\sin \theta \\ U_B=R{i}_B+\left(L_0+L_1\cos 2\theta \right)\frac{\text{d}{i}_B}{\text{d}t}-L_1\sin 2\theta \frac{\text{d}{i}_A}{\text{d}t}-2L_1\left(i_A\sin 2\theta +i_B\cos 2\theta \right)+k_e{\omega }_r\cos \theta \end{array}$ (1)

$\{\begin{array}{l}{U}_A=R{i}_A+L\frac{\text{d}{i}_A}{\text{d}t}-k_e{\omega }_r\sin \theta \\ U_B=R{i}_B+L\frac{\text{d}{i}_B}{\text{d}t}+k_e{\omega }_r\cos \theta \end{array}$ (2)

其中 $U_A$ 、 $U_B$ 和 $i_A$ 、 $i_B$ 分别为A、B两相定子绕组端电压和电流， $R$ 为定子绕组内阻， $L_0$ 、 $L_1$ 为定子绕组自感平均分量和基波分量， $\theta$ 为转子实时位置， $k_e$ 为步进电机反电动势系数， ${\omega }_r$ 为转子实际速度。

步进电机机械运动方程如式(3)所示 [23] ：

$T_e=J\frac{\text{d}{\omega }_r}{\text{d}t}+B{\omega }_r+T_L=k_e\left(i_B\cos \theta -i_A\sin \theta \right)$ (3)

式中 $T_e$ 为电机电磁转矩， $J$ 为电机转动惯量， $B$ 为电机摩擦系数， $T_L$ 为负载转矩。

本文使用的步进电机参数数值如表1所示：

Table 1. Parameters of the stepper motor

表1. 步进电机参数

4. 控制算法设计与实现

电机组采用电流环-速度环-位置环三环结构结合PID控制器，设定合适参数即可得到较好的控制性能。为了实现LQR算法优化三环PID调参，设计控制系统结构如图4所示。本研究中，速度环采用PI控制，PI调节作为一种线性调节方法，是将参考值与实际值比较后生成的误差，通过比例(P)放大调节和积分(I)控制，最后将运算结果作为系统的调节量进行控制。PI控制在连续时域中的数学模型函数如式(4)所示。位置环和电流环采用P控制，数学模型如式(5)所示。为获得良好的轨迹追踪性能，本文将步进电机精准定位，也就是电机实际转角 ${\theta }_i$ 精准度，作为参考因素，即 $e={\theta }_d-{\theta }_i$ 。

Figure 4. Structure of the system

图4. 系统结构图

$u_{PI}\left(t\right)=K_{p}e\left(t\right)+\frac{1}{T_i}{\displaystyle \int e\left(t\right)\text{d}t}$ (4)

$u_P\left(t\right)=K_{p}e\left(t\right)$ (5)

式中 $u\left(t\right)$ 为调节量， $K_p$ 为比例系数， $e\left(t\right)$ 为偏差， $T_i$ 为积分时间常数。

为得到系统状态向量，建立经Park变换后的两相混合式步进电机d-q坐标数学模型如式(6)所示，忽略定位转矩并使直轴电流 $i_d=0$ ，可得瞬时转矩 $\tau =k_e{i}_q$ [24] [25] 。

$\{\begin{array}{l}\frac{\text{d}\theta }{\text{d}t}={\omega }_r\\ \frac{\text{d}\omega }{\text{d}t}=\frac{1}{J}\left(k_e{i}_q-B{\omega }_r\right)\\ \frac{\text{d}{i}_d}{\text{d}t}=\frac{1}{L}\left(U_d-R{i}_d+L{\omega }_r{i}_q\right)\\ \frac{\text{d}{i}_q}{\text{d}t}=\frac{1}{L}\left(U_q-R{i}_q-L{\omega }_r{i}_d-k_e{\omega }_r\right)\end{array}$ (6)

取系统状态变量为 $\theta$ (位置)， $\omega$ (速度)， $i_q$ (交轴电流)，状态向量设为：

$x={\left[\begin{array}{lllll}{i}_q\left(t\right)\hfill & \omega \left(t\right)\hfill & \theta \left(t\right)\hfill & {\displaystyle \int e\left(t\right)\text{d}t}\hfill & e\left(t\right)\hfill \end{array}\right]}^{\mathrm{T}}$

得到系统的线性方程如式(7)所示：

$\{\begin{array}{l}\stackrel{\dot{}}{x}=Ax+Bu\\ p=Cx\end{array}$ (7)

其中 $A=\left[\begin{array}{ccccc}-\frac{R}{L}& -\frac{k_e}{L}& 0& 0& 0\\ \frac{k_e}{J}& -\frac{B}{J}& 0& 0& 0\\ 0& 1& 0& 0& -1\\ 0& 0& 0& 0& 1\\ 0& -1& 0& 0& 0\end{array}\right]$ ， $B=\left[\begin{array}{c}\frac{1}{L}\\ 0\\ 0\\ 0\\ 0\end{array}\right]$ ， $C=\left[\begin{array}{ccccc}0& 0& 1& 0& 0\end{array}\right]$ 。

为得到系统最优控制，引入LQR算法，设计一个线性反馈控制器 $u=-Kx$ ，求解出反馈矩阵 $K$ 的值即可得到系统状态方程中各反馈系数的值，则可定义最小性能函数 [26] ，如式(8)所示：

$J={\displaystyle {\int }_0^{\infty }{x}^{\text{T}}}Qx+u^{\text{T}}Ru\text{d}t$ (8)

将 $u=-Kx$ 代入：

$J={\displaystyle {\int }_0^{\infty }{x}^{\text{T}}}\left(Q+K^{\text{T}}RK\right)x\text{d}t$ (9)

定义一个辅助常量矩阵 $P$ ，使：

$\frac{\text{d}}{\text{d}t}{x}^{\text{T}}Px=-\left[x^{\text{T}}Qx+u^{\text{T}}Ru\right]$ (10)

${\stackrel{\dot{}}{x}}^{\text{T}}Px+x^{\text{T}}P\stackrel{\dot{}}{x}+x^{\text{T}}Qx+x^{\text{T}}{K}^{\text{T}}RKx=0$ (11)

$A^{\text{T}}P+PA+Q+K^{\text{T}}RK-K^{\text{T}}{B}^{\text{T}}P-PBK=0$ (12)

设 $K=R^{-1}{B}^{\text{T}}P$ ，代入上式可得：

$A^{\text{T}}P+PA+Q-PB{R}^{-1}{B}^{\text{T}}P=0$ (13)

对于给定的 $\left(A,B,Q,R,N\right)$ 即可解出如式(13)所示黎卡提方程式的 $P$ ，从而得到反馈控制器的 $K$ ，使系统性能代价函数最小。选取合适的加权矩阵 $Q$ 、 $R$ 值 [27] ：

$R=1,Q=\left[\begin{array}{ccccc}50& 0& 0& 0& 0\\ 0& 2000& 0& 0& 0\\ 0& 0& 2000& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right],N=0.$

由 $\left[K,S,E\right]=lqr\left(A,B,Q,R,N\right)$ 求解出：

$K=\left[\begin{array}{ccccc}6.5942& 50.0180& 44.7102& -0.0224& -36.3580\end{array}\right]$

根据对应关系以及实验效果做出调整，求得 $K_{p1}=1250$ ， $K_{p2}=2.57$ ， $K_{p3}=0.55$ ， $K_i=1.46$ 。

5. 结果与分析

5.1. 仿真分析

为了验证系统实用性，研究利用可视化仿真工具Matlab Simulink (R2021a，MathWorks Inc., USA)对步进电机控制系统进行建模仿真 [28] ，将得到的反馈系数代入所建立的系统仿真模型中，在位置环输入电机理想位置曲线，将实际位置与之对比，观察系统追踪性能，得出系统位置跟踪性能如图5所示。由于优化前的位置追踪是根据仿真效果进行手动调节PID参数实现的，这种方法存在一定主观性和调节准确度不足，而经过LQR算法得出最优PID参数的实际位置曲线与预设的位置曲线大致相吻合，使系统位置追踪性能相较于未优化前具有一定的提升。

Figure 5. Curve of position

图5. 位置曲线

速度环与电流环仿真结果如图6所示，由于存在一定累积误差，因此电流环在安全范围内有较小波动，速度环在加速和减速阶段性能良好，在匀速阶段存在轻微振荡，但基本符合设定的速度曲线，证明系统的速度和电流性能良好。

为了分析系统在外加负载下电流环与速度环的性能，验证系统分别设置不加负载、施加负载T_L = 0.35 N∙m和T_L = 0.5 N∙m三种情况，观察速度和电流曲线，结果如图7所示。电流曲线中T_L = 0 N∙m作为对照组，观察到当T_L = 0.35 N∙m时，交轴实际电流幅值最高可达1.65 A且存在一定波动。当T_L = 0.5 N∙m时，交轴实际电流幅值最高可达2.94 A，并且波动较大。当逐渐增大负载时，速度曲线存在波动，且在速度达到最大值后波动幅度较大。因此可知当外加负载小于0.35 N∙m时系统稳定性能最佳。

Figure 6. Curve of speed and current

图6. 速度和电流曲线

Figure 7. Speed and current performance of the system under different loads

图7. 不同负载下系统的速度与电流性能

5.2. 试验验证

将优化后的控制器应用于步态训练装置中，并将步态周期内踝关节运动轨迹点坐标转化成电机轴的转动，以丝杆电机控制踏板机构水平方向运动，齿轮电机控制踏板垂直方向运动，踝关节电机带动踏板旋转，进行如图8所示的踝关节步态训练试验，得出LQR优化后踏板训练系统步态轨迹跟踪结果如图8所示。试验中踏板训练装置跟踪步态轨迹的结果与正常的步态轨迹基本吻合，其中横轴为踏板装置水平方向位移，纵轴为竖直方向位移。

Figure 8. Gait trajectory training

图8. 步态轨迹训练

为了得到系统的误差范围以及验证跟踪步态轨迹的可重复性，在相同参数的前提下，对踏板训练装置进行了5组实验，对其在每个预设轨迹点的位置进行分析，通过编码器测量并计算得到其实际位置，轨迹跟踪平均误差绝对值如表2所示。

Table 2. Average error of trajectory tracking

表2. 轨迹跟踪平均误差

由上表可以看出，五组踏板训练装置运行时在水平方向上的误差均值小于7 mm，竖直方向上误差均值小于10 mm，即装置跟踪步态轨迹是可重复的 [29] 。同时为了验证装置运行时的安全性，避免旋转角度过大对患者造成二次损伤，分析踝关节径向的旋转角误差，误差均小于1˚，即保证了安全性。

6. 结论

为了提高患者进行康复训练的有效性和安全性，本文提出一种基于LQR与PID算法的步态轨迹控制方案，推导并利用LQR算法整合步进伺服驱动系统中位置环–速度环–电流环三闭环PID控制参数，进行的仿真实验和样机试验的结果证明了该算法的可行性和有效性，避免了PID参数调整的盲目性。此外本文所研究的方法还可利用到系统移动平台中，进一步对整体装置的移动轨迹进行追踪。

基金项目

国家自然科学基金项目(61473193)，上海市“科技创新行动计划”项目(21S31906000)，上海市“科技创新行动计划”产学研医合作领域项目(15DZ1941902)，上海理工大学医工交叉项目(10-19-308-505)。

参考文献

NOTES

^*通讯作者。