CN104635738A - 不确定康复步行训练机器人的精确轨迹跟踪最优控制方法 - Google Patents

Abstract

本发明公开一种不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，建立不确定康复步行训练机器人系统的动力学模型，构造一阶低通滤波器对影响康复步行训练机器人轨迹跟踪精度的不确定性进行整体估计，基于精确反馈线性化方法设计控制器，建立系统的线性化模型；以轨迹跟踪误差、速度跟踪误差和能量二次型为基础，设计康复步行训练机器人系统的二次型性能指标，采用优化控制方法，实现康复步行训练机器人的精确轨迹跟踪并使性能指标最优；本发明将输出高精度的PWM信号提供给电机驱动单元，使机器人实现对参考轨迹信号的精确跟踪。该控制方法无须知道系统不确定性的界限，可以提高康复步行训练机器人轨迹跟踪精度并使系统性能达到最优。

Description

不确定康复步行训练机器人的精确轨迹跟踪最优控制方法

技术领域：

本发明属于轮式康复机器人的控制领域，尤其是关于不确定康复步行训练机器人的精确轨迹跟踪最优控制方法。

背景技术：

康复步行训练机器人需要精确跟踪医生指定的训练轨迹，才能达到满意的康复效果。然而，由于康复者质量及系统偏心距带来的不确定性，严重影响轨迹跟踪精度，因此，研究提高轨迹跟踪精度并实现系统性能的最优控制方法具有重要意义。本发明将康复步行训练机器人系统的不确定性视为整体，通过构建一阶低通滤波器对其进行估计，从而无须知道系统不确定性的界限，避免了界限的不准确性对轨迹跟踪精度的影响。本发明在提高轨迹跟踪精度的同时，又能使系统性能达到最优，对于康复步行训练机器人高性能控制系统的设计具有重要参考价值。

发明内容：

发明目的：

为了解决上述问题，本发明提供了一种不确定康复步行训练机器人的精确轨迹跟踪最优控制方法。

技术方案：

本发明是通过以下技术方案来实现的：

一种不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，其特征在于：建立不确定康复步行训练机器人系统的动力学模型，构造一阶低通滤波器对影响康复步行训练机器人轨迹跟踪精度的不确定性进行整体估计，基于精确反馈线性化方法设计控制器，建立系统的线性化模型；以轨迹跟踪误差、速度跟踪误差和能量二次型为基础，设计康复步行训练机器人系统的二次型性能指标，采用优化控制方法，实现康复步行训练机器人的精确轨迹跟踪并使性能指标最优；具体步骤如下：

1)建立不确定康复步行训练机器人系统的动力学模型，构造一阶低通滤波器对影响康复步行训练机器人轨迹跟踪精度的不确定性进行整体估计，基于精确反馈线性化方法设计控制器，建立系统的线性化模型；

2)以轨迹跟踪误差、速度跟踪误差和能量二次型为基础，设计康复步行训练机器人系统的二次型性能指标，采用优化控制方法，实现康复步行训练机器人的精确轨迹跟踪并使性能指标最优；

3)基于STM32系列单片机将输出高精度的PWM信号提供给电机驱动单元，使机器人实现对参考轨迹信号的精确跟踪。

步骤1)中，首先，建立系统的动力学模型：

$M_0\stackrel{\·\·}{X}=B\left(\mathrm{\θ}\right)u\left(t\right)$

$M_0=\left[\begin{array}{ccc}M+m& 0& 0\\ 0& M+m& 0\\ 0& 0& I_0+m{r}_0^2\end{array}\right],X=\left[\begin{array}{c}x\left(t\right)\\ y\left(t\right)\\ \mathrm{\θ}\left(t\right)\end{array}\right],B\left(\mathrm{\θ}\right)=\left[\begin{array}{cccc}-\sin \mathrm{\θ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}& \cos \mathrm{\θ}\\ \cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ L& -L& -L& L\end{array}\right],u\left(t\right)=\left(\begin{array}{c}{f}_1\\ f_2\\ f_3\\ f_4\end{array}\right)$

其中M₀为包含机器人质量M、康复者质量m和转动惯量I₀的系数矩阵，r₀为系统偏心距，X为机器人的实际行走轨迹，B(θ)为机器人旋转角度构成的系数矩阵，u(t)表示机器人的控制输入力，θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角。

因康复者质量m及偏心距r₀大大影响系统轨迹跟踪精度，将其视为系统的不确定性d；分解M₀为仅含机器人质量M和转动惯量I₀的确定系数矩阵M₁及含有r₀和m结合的不确定系数矩阵ΔM₁，建立不确定康复步行训练机器人系统的动力学模型：

$\stackrel{\·\·}{X}={M_1}^{-1}B\left(\mathrm{\θ}\right)u\left(t\right)+d$

其中 $d=(M_0^{-1}-M_1^{-1})B\left(\mathrm{\θ}\right)u\left(t\right)={\mathrm{\ΔM}}_1^{-1}B\left(\mathrm{\θ}\right)u\left(t\right).$

设计控制器其中构造一阶低通滤波器G(s)对不确定性d进行整体估计

$\hat{d}=G\left(s\right)[\stackrel{\·\·}{X}-{\stackrel{\·\·}{X}}_d-v\left(t\right)-v_d\left(t\right)]$

其中τ为时间常数；为抵消不确定性d对系统跟踪精度的影响，设计 $v_d\left(t\right)=-\hat{d},$ 于是得到

$v_d\left(t\right)=\frac{1}{\mathrm{\τs}}(\stackrel{\·\·}{X}-{\stackrel{\·\·}{X}}_d-v\left(t\right))=\frac{1}{\mathrm{\τ}}\∫[\stackrel{\·\·}{X}-{\stackrel{\·\·}{X}}_d-v\left(t\right)]\mathrm{dt}.$

根据不确定康复机器人实际行走轨迹X和医生指定的训练轨迹X_d的跟踪误差e(t)，得到如下形式的误差状态模型：

$\stackrel{\·\·}{e}\left(t\right)=v\left(t\right)$

建立系统的线性化模型为

$\stackrel{\·}{x}\left(t\right)=\mathrm{Ax}\left(t\right)+\mathrm{Bv}\left(t\right)$

其中 $A=\left(\begin{array}{cc}0& I\\ 0& 0\end{array}\right),B=\left(\begin{array}{c}0\\ I\end{array}\right),x\left(t\right)=\left(\begin{array}{c}e\left(t\right)\\ \stackrel{\·}{e}\left(t\right)\end{array}\right).$

步骤2)中，基于误差状态模型，设计系统的二次型性能指标为：

$J={\∫}_0^{\∞}[\frac{1}{2}{x}^T\left(t\right)\mathrm{Qx}\left(t\right)+\frac{1}{2}{v}^T\left(t\right)\mathrm{Rv}\left(t\right)]\mathrm{dt}.$

基于最优控制理论得到控制率v(t)为：

v(t)＝-R^-1B^TPx(t)

其中R为正常数， $P=\left[\begin{array}{cc}{P}_{11}& P_{12}^T\\ P_{12}& P_{22}\end{array}\right],$ 且满足PA+A^TP-PBR^-1B^TP＝Q；

进一步，控制率 $v\left(t\right)={-R}^{-1}{P}_{12}e\left(t\right)-R^{-1}{P}_{22}\stackrel{\·}{e}\left(t\right).$

采用优化控制方法，使系统性能指标最优并完成精确轨迹跟踪控制，将v(t)代入u(t)和v_d(t)，得到不确定康复步行训练机器人系统的控制器为：

步骤3)中，以STM32系列单片机为主控制器，主控制器的输入接电机测速模块，输出接电机驱动模块，电机驱动电路与直流电机相连，电源系统给各个电气设备供电。

主控制器的控制方法为通过读取电机编码器的反馈信号与主控制器给定的控制命令信号X_d对比得出误差信号，根据误差信号，主控制器按照预定的控制算法计算出电机的控制量，送给电机驱动单元，电机转动带动轮子维持自身平衡及按指定方式运动。

优点及效果：

本发明是一种不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，具有如下优点：

本发明提供的控制方法无须知道系统不确定性的界限，可以提高康复步行训练机器人的轨迹跟踪精度并使系统性能达到最优。

本发明通过构建一阶低通滤波器对系统的不确定性进行整体估计，设计方法简单且易于实现，增强了控制器处理系统不确定性的鲁棒能力，从而可实现康复步行训练机器人对指定轨迹的高精度跟踪。

附图说明：

图1为本发明控制器工作框图；

图2为本发明的STM32F101单片机最小系统示意图；

图3为本发明的主控制器外围扩展电路图；

图4为本发明硬件总体原理电路图。

具体实施方式：

下面结合附图对本发明做进一步的说明，但本发明的保护范围不受实施例的限制。

本发明是一种不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，建立不确定康复步行训练机器人系统的动力学模型，构造一阶低通滤波器对影响康复步行训练机器人轨迹跟踪精度的不确定性进行整体估计，基于精确反馈线性化方法设计控制器，建立系统的线性化模型；以轨迹跟踪误差、速度跟踪误差和能量二次型为基础，设计康复步行训练机器人系统的二次型性能指标，采用优化控制方法，实现康复步行训练机器人的精确轨迹跟踪并使性能指标最优。具体步骤如下：

1)建立不确定康复步行训练机器人系统的动力学模型，构造一阶低通滤波器对影响康复步行训练机器人轨迹跟踪精度的不确定性进行整体估计，基于精确反馈线性化方法设计控制器，建立系统的线性化模型；

2)以轨迹跟踪误差、速度跟踪误差和能量二次型为基础，设计康复步行训练机器人系统的二次型性能指标，采用优化控制方法，实现康复步行训练机器人的精确轨迹跟踪并使系统性能指标最优；

3)基于STM32系列单片机将输出高精度的PWM信号提供给电机驱动单元，使机器人实现对参考轨迹信号的精确跟踪。

步骤1)中，基于康复步行训练机器人的特征，首先，建立系统的动力学模型，步骤如下：

$M_0\stackrel{\·\·}{X}=B\left(\mathrm{\θ}\right)u\left(t\right)---\left(1\right)$

$M_0=\left[\begin{array}{ccc}M+m& 0& 0\\ 0& M+m& 0\\ 0& 0& I_0+m{r}_0^2\end{array}\right],X=\left[\begin{array}{c}x\left(t\right)\\ y\left(t\right)\\ \mathrm{\θ}\left(t\right)\end{array}\right],B\left(\mathrm{\θ}\right)=\left[\begin{array}{cccc}-\sin \mathrm{\θ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}& \cos \mathrm{\θ}\\ \cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ L& -L& -L& L\end{array}\right],u\left(t\right)=\left(\begin{array}{c}{f}_1\\ f_2\\ f_3\\ f_4\end{array}\right)$

其中M₀为包含机器人质量M、康复者质量m和转动惯量I₀的系数矩阵，X为机器人的实际行走轨迹，B(θ)为机器人旋转角度构成的系数矩阵，u(t)表示机器人的控制输入力，θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角。

因康复者质量m及偏心距r₀大大影响系统轨迹跟踪精度，将其视为系统的不确定性d。分解M₀为仅含机器人质量M和转动惯量I₀的确定系数矩阵M₁及含有r₀和m结合的不确定系数矩阵ΔM₁，建立不确定康复步行训练机器人系统的动力学模型：

$\stackrel{\·\·}{X}={M_1}^{-1}B\left(\mathrm{\θ}\right)u\left(t\right)+d---\left(2\right)$

其中 $d=(M_0^{-1}-M_1^{-1})B\left(\mathrm{\θ}\right)u\left(t\right)={\mathrm{\ΔM}}_1^{-1}B\left(\mathrm{\θ}\right)u\left(t\right).$

基于不确定康复步行训练机器人系统的动力学模型，结合精确反馈线性化理论，设计控制器其中构造一阶低通滤波器G(s)对不确定性d进行整体估计

$\hat{d}=G\left(s\right)[\stackrel{\·\·}{X}-{\stackrel{\·\·}{X}}_d-v\left(t\right)-v_d\left(t\right)]---\left(3\right)$

其中τ为时间常数，G(s)能保证为抵消不确定性d对系统跟踪精度的影响，设计于是得到

$v_d\left(t\right)=\frac{1}{\mathrm{\τs}}(\stackrel{\·\·}{X}-{\stackrel{\·\·}{X}}_d-v\left(t\right))=\frac{1}{\mathrm{\τ}}\∫[\stackrel{\·\·}{X}-{\stackrel{\·\·}{X}}_d-v\left(t\right)]\mathrm{dt}---\left(4\right).$

使系统实现精确轨迹跟踪的最优控制方法，根据不确定康复机器人实际行走轨迹X和医生指定的训练轨迹X_d的跟踪误差e(t)，将控制器u(t)代入动力学模型(2)，得到如下形式的误差状态模型：

$\stackrel{\·\·}{e}\left(t\right)=v\left(t\right)---\left(5\right);$

根据式(5)，建立系统的线性化模型为

$\stackrel{\·}{x}\left(t\right)=\mathrm{Ax}\left(t\right)+\mathrm{Bv}\left(t\right)---\left(6\right);$

其中 $A=\left(\begin{array}{cc}0& I\\ 0& 0\end{array}\right),B=\left(\begin{array}{c}0\\ I\end{array}\right),x\left(t\right)=\left(\begin{array}{c}e\left(t\right)\\ \stackrel{\·}{e}\left(t\right)\end{array}\right).$

步骤2)以轨迹跟踪误差、速度跟踪误差和能量二次型为基础，建立不确定康复步行训练机器人系统的二次型性能指标，具体过程如下：基于误差状态模型，设计系统的二次型性能指标为：

$J={\∫}_0^{\∞}[\frac{1}{2}{x}^T\left(t\right)\mathrm{Qx}\left(t\right)+\frac{1}{2}{v}^T\left(t\right)\mathrm{Rv}\left(t\right)]\mathrm{dt}---\left(7\right).$

采用优化控制方法，使系统性能达到最优的情况下完成精确轨迹跟踪控制，基于最优控制理论得到控制率v(t)为

v(t)＝-R^-1B^TPx(t)   (8)；

其中R为正常数， $P=\left[\begin{array}{cc}{P}_{11}& P_{12}^T\\ P_{12}& P_{22}\end{array}\right],$ 且满足PA+A^TP-PBR^-1B^TP＝Q；

进一步，控制率 $v\left(t\right)=-R^{-1}{P}_{12}e\left(t\right)-R^{-1}{R}_{22}\stackrel{\·}{e}\left(t\right).$

以轨迹跟踪误差、速度跟踪误差和能量二次型为基础，设计康复步行训练机器人系统的二次型性能指标，采用优化控制方法，使系统性能达到最优的情况下完成精确轨迹跟踪控制,将v(t)代入u(t)和v_d(t)，得到不确定康复步行训练机器人系统的控制器为：

步骤3)基于STM32系列单片机(如图2所示)将输出高精度的PWM信号提供给电机驱动单元，使机器人实现对参考轨迹信号的跟踪，以STM32系列单片机为主控制器，主控制器的输入接电机测速模块、输出接电机驱动模块；电机驱动电路与直流电机相连；电源系统给各个电气设备供电，如图3所示。

控制器的工作框图如图1所示，其控制方法为通过读取电机编码器的反馈信号与主控制器给定的控制命令信号X_d对比得出误差信号，根据误差信号，主控制器按照预定的控制算法计算出电机的控制量，送给电机驱动单元，电机转动带动轮子维持自身平衡及按指定方式运动。

结论：

本发明解决了康复者和康复者引起系统重心偏移产生的不确定性影响康复步行训练机器人轨迹跟踪精度问题，可以通过构建一阶低通滤波器和精确反馈线性化方法，有效的提高轨迹跟踪精度并使系统性能达到最优。

Claims (10)

1.一种不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，其特征在于：建立不确定康复步行训练机器人系统的动力学模型，构造一阶低通滤波器对影响康复步行训练机器人轨迹跟踪精度的不确定性进行整体估计，基于精确反馈线性化方法设计控制器，建立系统的线性化模型；以轨迹跟踪误差、速度跟踪误差和能量二次型为基础，设计康复步行训练机器人系统的二次型性能指标，采用优化控制方法，实现康复步行训练机器人的精确轨迹跟踪并使性能指标最优；具体步骤如下：

1)建立不确定康复步行训练机器人系统的动力学模型，构造一阶低通滤波器对影响康复步行训练机器人轨迹跟踪精度的不确定性进行整体估计，基于精确反馈线性化方法设计控制器，建立系统的线性化模型；

2)以轨迹跟踪误差、速度跟踪误差和能量二次型为基础，设计康复步行训练机器人系统的二次型性能指标，采用优化控制方法，实现康复步行训练机器人的精确轨迹跟踪并使性能指标最优；

3)基于STM32系列单片机将输出高精度的PWM信号提供给电机驱动单元，使机器人实现对参考轨迹信号的精确跟踪。

2.根据权利要求1所述的不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，其特征在于：步骤1)中，首先，建立系统的动力学模型：

$M_0\stackrel{\·\·}{X}=B\left(\mathrm{\θ}\right)u\left(t\right)$

$M_0=\left[\begin{array}{ccc}M+m& 0& 0\\ 0& M+m& 0\\ 0& 0& I_0+{\mathrm{mr}}_0^2\end{array}\right],X=\left[\begin{array}{c}x\left(t\right)\\ y\left(t\right)\\ \mathrm{\θ}\left(t\right)\end{array}\right],B\left(\mathrm{\θ}\right)\left[\begin{array}{cccc}-\sin \mathrm{\θ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}& \cos \mathrm{\θ}\\ \cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ L& -L& -L& L\end{array}\right],u\left(t\right)=\left(\begin{array}{c}{f}_1\\ f_2\\ f_3\\ f_4\end{array}\right)$

其中M₀为包含机器人质量M、康复者质量m和转动惯量I₀的系数矩阵，r₀为系统偏心距，X为机器人的实际行走轨迹，B(θ)为机器人旋转角度构成的系数矩阵，u(t)表示机器人的控制输入力，θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角。

3.根据权利要求2所述的不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，其特征在于：因康复者质量m及偏心距r₀大大影响系统轨迹跟踪精度，将其视为系统的不确定性d；分解M₀为仅含机器人质量M和转动惯量I₀的确定系数矩阵M₁及含有r₀和m结合的不确定系数矩阵ΔM₁，建立不确定康复步行训练机器人系统的动力学模型：

$\stackrel{\·\·}{X}={M_1}^{-1}B\left(\mathrm{\θ}\right)u\left(t\right)+d$

其中 $d=(M_0^{-1}-M_1^{-1})B\left(\mathrm{\θ}\right)u\left(t\right)={\mathrm{\ΔM}}_1^{-1}B\left(\mathrm{\θ}\right)u\left(t\right).$

4.根据权利要求3所述的不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，其特征在于：设计控制器其中构造一阶低通滤波器G(s)对不确定性d进行整体估计

$\hat{d}=G\left(s\right)[\stackrel{\·\·}{X}-{\stackrel{\·\·}{X}}_d-v\left(t\right)-v_d\left(t\right)]$

其中τ为时间常数；为抵消不确定性d对系统跟踪精度的影响，设计于是得到

$v_d\left(t\right)=\frac{1}{\mathrm{\τs}}(\stackrel{\·\·}{X}-{\stackrel{\·\·}{X}}_d-v\left(t\right))=\frac{1}{\mathrm{\τ}}\∫[\stackrel{\·\·}{X}-{\stackrel{\·\·}{X}}_d-v\left(t\right)]\mathrm{dt}.$

5.根据权利要求4所述的不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，其特征在于：根据不确定康复机器人实际行走轨迹X和医生指定的训练轨迹X_d的跟踪误差e(t)，得到如下形式的误差状态模型：

$\stackrel{\·\·}{e}\left(t\right)=v\left(t\right)$

建立系统的线性化模型为

$\stackrel{\·}{x}\left(t\right)=\mathrm{Ax}\left(t\right)+\mathrm{Bv}\left(t\right)$

其中 $A=\left(\begin{array}{cc}0& I\\ 0& 0\end{array}\right),B=\left(\begin{array}{c}0\\ I\end{array}\right),x\left(t\right)=\left(\begin{array}{c}e\left(t\right)\\ \stackrel{\·}{e}\left(t\right)\end{array}\right).$

6.根据权利要求1所述的不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，其特征在于：步骤2)中，基于误差状态模型，设计系统的二次型性能指标为：

$J={\∫}_0^{\∞}[\frac{1}{2}{x}^T\left(t\right)\mathrm{Qx}\left(t\right)+\frac{1}{2}{v}^T\left(t\right)\mathrm{Rv}\left(t\right)]\mathrm{dt}.$

7.根据权利要求6所述的不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，其特征在于：基于最优控制理论得到控制率v(t)为：

v(t)＝-R^-1B^TPx(t)

其中R为正常数， $P=\left[\begin{array}{cc}{P}_{11}& P_{12}^T\\ P_{12}& P_{22}\end{array}\right],$ 且满足PA+A^TP-PBR^-1B^TP＝Q；

进一步，控制率 $v\left(t\right)=-R^{-1}{P}_{12}e\left(t\right)-R^{-1}{P}_{22}\stackrel{\·}{e}\left(t\right).$

8.根据权利要求7所述的不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，其特征在于：采用优化控制方法，使系统性能指标最优并完成精确轨迹跟踪控制，将v(t)代入u(t)和v_d(t)，得到不确定康复步行训练机器人系统的控制器为：

9.根据权利要求1所述的不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，其特征在于：步骤3)中，以STM32系列单片机为主控制器，主控制器的输入接电机测速模块，输出接电机驱动模块，电机驱动电路与直流电机相连，电源系统给各个电气设备供电。

10.根据权利要求1或9所述的不确定康复步行训练机器人的精确轨迹跟踪最优控制方法，其特征在于：主控制器的控制方法为通过读取电机编码器的反馈信号与主控制器给定的控制命令信号X_d对比得出误差信号，根据误差信号，主控制器按照预定的控制算法计算出电机的控制量，送给电机驱动单元，电机转动带动轮子维持自身平衡及按指定方式运动。