一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法

1.本发明属于机器人控制技术领域，尤其涉及一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法。


背景技术：

2.由于老龄化人口和下肢残疾人口有一定的行走功能障碍，在平时的生活中时常发生摔倒，有研究表明，老年人每天进行行走练习不仅可以预防摔倒的发生，还能增加他们身体的稳定性。越来越多的步行训练机器人被广泛应用。全方位步行训练机器人可以在不改变姿态的情况下向任何方向移动。它非常适合在狭窄的空间中工作，已被广泛研究。
3.利用步行训练机器人辅助老人生活，关键的是安全性。步行训练机器人行走时为避免撞击周围的人和物，在室内全局或局部路径规划好的轨迹上，必须具备有高精度的轨迹跟踪能力。但是步行训练机器人在室内使用时，由于使用者姿态的改变，导致机器人的重心与中心位置不重合，且重心偏移是时变的。考虑重心偏移影响的轨迹跟踪控制的常用方法有pid控制，模型预测控制，自适应控制等。对于给定的目标轨迹，pid控制法在一定的地面条件，即当重心偏移一定时，通过调节其控制参数，可以达到一定的追踪精度。但是当改变轨迹，或者使用者发生变化，即重心偏移实时变化时，需要重新调节pid控制参数，否则将不能保证轨迹跟踪的精度。因此说用pid控制实现步行训练机器人轨迹跟踪时，很难实时对应各种轨迹以及适应各种不同的重心偏移变化。模型预测控制，可在满足约束条件下减小重心偏移对跟踪精度的影响，但未直面重心偏移引起的动态耦合矩阵不确定问题进行处理，所以模型预测控制不容易实现步行训练机器人的实时轨迹跟踪控制。自适应控制法不需要实时得到当前时刻重心偏移量，但其稳定性是建立在重心偏移变化比较慢的基础上，重心位置变化快会导致该控制算法不稳定。目前，全方位步行训练机器人的轨迹跟踪问题是国内外研究的难点问题。


技术实现要素：

4.针对现有技术的不足，本发明设计一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法。
5.一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法，包括以下步骤：
6.步骤1：步行训练机器人全方位移动部件为四个万向轮，对步行训练机器人进行物理建模，使用者对步行训练机器人的重心偏移影响的运动学模型和动力学模型分别为：
7.[0008][0009]
其中，θ为机器人的姿势角，分别指机器人中心位置在x轴和y轴方向的分速度,为机器人自旋的角速度，fi为作用在各个万向轮上的驱动力，vi为各个万向轮速度，m为机器人的质量，m为人附加给机器人的等价质量，i为机器人的转动惯量，l是机器人中心到四个全向轮的距离，r为中心到重心间距离，α是重心与中心连接线相对与姿势角的角度，为机器人中心位置在x方向与y方向的加速度,为机器人旋转的角加速度；l1＝l-rcosα，l2＝-l rsinα，l3＝-l-rcosα，l4＝-l rsinα为机器人重心处到四个轮子的距离；
[0010]
当
[0011]
时，
[0012]
动力学模型即式2以表示；
[0013]
步骤2：假设在kτ时刻之后，又经过非常微小时间后的时刻为kτ

；在时刻t＝kτ

时，作用于步行训练机器人万向轮上的驱动力为f(kτ

)，在一个采样周期τ内作用于各个万向轮驱动力为不变的，则时刻t＝kτ和时刻t＝kτ

的步行训练机器人的离散型动力学模型分别为：
[0014][0015][0016]
其中f[(k-1)τ

]为时间区间[(k-1)τ

,kτ]内作用于各个万向轮的输入驱动力，f[kτ

]是经过一个采样周期τ后，下一个时间区间[kτ

,(k 1)τ]内的作用于各个万向轮的驱动力；为kg的广义逆，为时间区间[(k-1)τ

,kτ]内轮椅中心处加速度值，为时间区间[kτ

,(k 1)τ]内轮椅中心处加速度值，为t＝kτ时轮椅中心处速度值，为t＝kτ

时轮椅中心处速度值；
[0017]
步骤3：把xd(t)作为步行训练机器人的目标轨迹，为了使步行训练机器人跟踪目标轨迹，基于时刻t＝kτ的万向轮驱动力为f[(k-1)τ

]，则在时刻t＝kτ

，基于数值加速度控制，假设mg和t均已知，作用在各个万向轮上的驱动力f[kτ

]的计算式如下：
[0018][0019]
其中，xd(kτ

)为t＝kτ

时刻目标轨迹xd的值，为t＝kτ

时刻目标速度的值，为t＝kτ

时刻目标加速度的值，xc(kτ)为t＝kτ时刻机器人位置xc的值，k
p
为位置偏差系数，k
p
＝diag(k
px
,k
py
,k
pθ
)，k
px
,k
py
,k
pθ
为在x轴y轴以及姿态角方向的位置偏差系数；kd为速度偏差系数kd＝diag(k
dx
,k
dy
,k
dθ
)k
dx
,k
dy
,k
dθ
为在x轴y轴以及姿态角方向的速度偏差系数；
[0020]
步骤4：对轮椅机器人的重心偏移参数r,α进行估计；
[0021]
步骤4.1：采用长短期记忆神经网络建立轮椅机器人实际对比模型位置变化量与当前时刻驱动力关系；
[0022]
步骤4.1.1：使机器人空载按预定的轨迹移动，收集机器人四个轮子的驱动力记为f，机器人从t＝(k-1)τ时刻到t＝kτ时刻位置变化量数据记为δx
′c(k)；
[0023]
步骤4.1.2：对步骤4.1.1中采集的数据进行预处理；
[0024]
步骤4.1.3：设计隐藏层个数，构建长短期记忆神经网络；
[0025]
步骤4.1.4：确定最优的长短期记忆神经网络：δx
′c＝f(f)；
[0026]
步骤4.2：理想对比模型位置变化量与当前时刻驱动力关系如下等式；
[0027][0028]
其中，δx
′c(k)，δy
′c(k)为理想对比模型从t＝(k-1)τ时刻到t＝kτ时刻在x，y方向的位置变化量，δθ
′
(k)为理想对比模型从t＝(k-1)τ时刻到t＝kτ时刻的姿态角变化量，为t＝(k-1)τ时刻对比模型在x，y方向的速度，为t＝(k-1)τ时刻对比模型的旋转角速度，fi(k)为第i个全向轮t＝kτ时刻受到的驱动力，为矩阵mc的逆，θ
′
k-1
为t＝(k-1)τ时刻对比机器人模型的姿态角，τ为采样时间；
[0029]
当
[0030]
时，
[0031]
机器人从t＝(k-1)τ时刻到t＝kτ时刻位置变化量和驱动力之间关系以表示；
[0032]
步骤4.3：实际机器人从t＝(k-1)τ时刻到t＝kτ时刻位置变化量与当前时刻驱动力关系如下等式；
[0033][0034]
其中，δxc(k)，δyc(k)为从t＝(k-1)τ时刻到t＝kτ时刻机器人在x，y方向的位置变化量，δθ(k)为从t＝(k-1)τ时刻到t＝kτ时刻机器人的姿态角变化量，为t＝(k-1)τ时刻机器人重心处在x，y方向的速度，为t＝(k-1)τ时刻机器人的旋转角速度，θ
k-1
为t＝(k-1)τ时刻机器人的姿态角，rk,αk为t＝kτ时刻机器人的重心偏移量r,α的值；
[0035]
当
[0036][0037]
时，
[0038]
实际机器人从t＝(k-1)τ时刻到t＝kτ时刻位置变化量与当前时刻驱动力关系以表示；t-1
(k)为矩阵t(k)的逆，为矩阵mg(k)的逆；
[0039]
步骤4.4：根据式6和式7得对比模型和实际机器人之间从t＝(k-1)τ时刻到t＝kτ时刻的位置变化差与当前时刻的重心偏移量关系如下等式；
[0040][0041]
[0042][0043]
其中：其中δ0x(k)为从t＝(k-1)τ时刻到t＝kτ时刻实际机器人和对比机器人在x方向位置变化量的差值，δ0y(k)为从t＝(k-1)τ时刻到t＝kτ时刻实际机器人和对比机器人在y方向位置变化量的差值，δ0θ(k)为从t＝(k-1)τ时刻到t＝kτ时刻实际机器人和对比机器人姿态角变化量的差值。δu
cx
为从t＝(k-1)τ时刻到t＝kτ时刻机器人中心处的转矩在x方向的分量，δu
cy
为从t＝(k-1)τ时刻到t＝kτ时刻机器人中心处的转矩在y方向的分量，δu
cθ
为从t＝(k-1)τ时刻到t＝kτ时刻机器人中心处的旋转力矩；
[0044]
a1＝-r
k2
sin2(αk θ
k-1
)δu
cx
(k)-rksin(αk θ
k-1
)δu
cθ
(k) r
k2
sin(αk θ
k-1
)cos(αk θ
k-1
)δu
cy
(k)
[0045]
a2＝-r
k2
cos2(αk θ
k-1
)δu
cy
(k) rkcos(αk θ
k-1
)δu
cθ
(k) rkcos(αk θ
k-1
)sin(αk θ
k-1
)δu
cx
(k)
[0046]
a3＝-rksin(αk θ
k-1
)δu
cx
(k) mr
k2-rkcos(αk θ
k-1
)δu
cy
(k)
[0047]
当，时，
[0048]
根据式8—式10得到下式：
[0049][0050]
步骤4.5：采用牛顿迭代法求解式8—式10；
[0051]
步骤4.5.1：选定rk，αk的初始值，计算雅克比矩阵并求得雅克比矩阵的初始值f0′
(rk,αk)，f0′
(rk,αk)-1
；
[0052]
步骤4.5.2：采用r
k 1
＝r
k-f
′
(rk)-1f′
(rk)，α
k 1
＝α
k-f
′
(αk)-1f′
(αk)进行雅克比迭代；
[0053]
步骤4.5.3：当两次迭代的差值的绝对值小于10-7
时，结束迭代；
[0054]
步骤5：根据步骤4中得到的重心偏移参数估计值，对重心偏移参数进行预测，步骤如下：
[0055]
步骤5.1：采用张神经网络预测求逆方法对重心偏移参数进行预测，公式如下：
[0056][0057]
其中h为设计参数，为采用张神经网络估计出的t＝(k 1)τ时刻的值，kg(k)，kg(k-1)，kg(k-2)，kg(k-3)分别为t＝kτ，t＝(k-1)τ，t＝(k-2)τt＝(k-3)τ时刻kg的值。分别为t＝kτ，t＝(k-1)τ，t＝(k-2)τt＝(k-3)τ时刻的值；
[0058]
步骤5.2：由式12得到的从而求得k 1时刻重心偏移量rk 1，αk 1的估计值；
[0059][0060]
其中，为t＝(k 1)τ时刻r，α的估计值，为t＝(k 1)τ时刻l1，l2的估计值；
[0061]
步骤6：将步骤5中得到的k 1时刻的重心偏移量r
k 1
,α
k 1
带入数值加速度控制器中对机器人进行控制；
[0062][0063]
其中，为mg，t的估计值；
[0064]
步骤7：决定k
p
和kd的最优值，将目标轨迹设为xd，步行训练机器人跟踪误差定义为e(t)＝xd(t)-xc(t)，当作用在步行训练机器人万向轮上的驱动力瞬间变化时，其步行训练机器人加速度也将瞬间变化，而步行训练机器人速度和位置是不会发生瞬间变化，又由步行训练机器人的离散型动力学模型和作用于各个万向轮上的驱动力的计算式，得到步行训练机器人跟踪误差的离散模型如下：
[0065][0066]
其中e(kτ

)＝xd(kτ

)-xc(kτ)为为t＝kτ时刻机器人的速度误差，为t＝kτ时刻机器人的速度误差，为t＝kτ时刻机器人加速度误差；
[0067]
在时间区间[kτ

,(k 1)τ]内，
[0068][0069][0070]
其中为机器人速度误差，为机器人加速度误差，为t＝(k 1)τ时刻的值，为t＝kτ时刻的值，为t＝kτ时刻的值，e[(k 1)τ]为t＝(k 1)τ时刻e(t)的值，e[kτ]为t＝kτ时刻e(t)的值；
[0071]
根据式15—式17得到下式：
[0072]
[0073]
令
[0074]
以及
[0075]
得到e[(k 1)τ]＝ae[kτ]；
[0076]
对于上式的误差反馈系统，用极点配置法则能选出k
p
和kd的最优值，然后将其代入公式14中，完成对机器人的轨迹跟踪控制。
[0077]
本发明有益技术效果：
[0078]
本发明着眼于作用在各个万向轮上的驱动力和步行训练机器人位置之间的瞬间关系，设计参数估计及预测方法，实时估计并预测重心偏移量，然后采用基于数值加速度控制的步行训练机器人的轨迹跟踪控制方法。所设计的离散数值加速度控制器算法可以抵消步行训练机器人系统实时运动过程中存在的非线性时变干扰。基于模型参考法对重心偏移参数进行估计，再基于张神经网络对估计的参数进行预测，可解决估计滞后性的影响，使得位置偏差收敛到参考轨迹。
[0079]
本发明的重心参数预测方法，使步行训练机器人的轨迹跟踪精度得到进一步有效提高。本发明易于计算机实时控制的实现，在机器人运行过程中重心偏移实时变化情况下也能实现高精度的轨迹跟踪，具有良好的鲁棒性。
附图说明
[0080]
图1为本发明的步行训练机器人的结构示意图。
[0081]
图2为本发明中步行训练机器人万向轮的坐标配置图。
[0082]
图3为本发明中步行训练机器人控制系统的框图。
[0083]
图4为本发明中长短期记忆神经网络建立对比模型的原理图。
[0084]
图5为本发明中长短期记忆神经网络输入输出数据图。
[0085]
图6为本发明中参数预测原理图。
[0086]
图7为本发明中实施例1轨迹a的仿真实验结果图。
[0087]
图8为本发明中实施例2轨迹a的仿真实验结果图。
[0088]
图9为本发明中实施例2轨迹a的参数估计及预测结果图。
[0089]
图10为本发明中实施例1轨迹b的仿真实验结果图。
[0090]
图11为本发明中实施例2轨迹b的仿真实验结果图。
具体实施方式
[0091]
下面将结合具体实施方式和说明书附图对本发明及其有益效果作进一步详细说明。
[0092]
本发明针对现有技术的不足，提供一种含有重心偏移影响的步行训练机器人轨迹跟踪方法，该方法首先基于牛顿-欧拉刚体学原理建立考虑重心偏移的动力学模型并基于长短期记忆神经网络训练构建其对比模型。其次基于模型对比法实时估计重心偏移参数，再基于张神经网络对估计的重心偏移参数进行预测以减小估计过程引起的滞后，最后基于非线性动态反馈解耦法设计数值加速度控制实现。另外，该方法的数值加速度控制器能很
好地处理步行训练机器人重心实时变化的影响，并且由构成的跟踪误差状态反馈形式，用极点配置法可以选出数值控制器中的最优调节参数，轨迹跟踪精度将得到进一步有效提高。
[0093]
本实施例提供一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法，包括以下步骤：
[0094]
步骤1：步行训练机器人全方位移动部件为四个万向轮，当步行训练机器人的质量m＝80kg，步行训练机器人的转动惯量i＝0.7kg
·
m，驱动力相对机器人中心位置的力臂长度l＝0.6m时，图1所示的步行训练机器人，按图2所示的万向轮的坐标配置，其运动学模型和动力学模型以及重心偏移模型如下：
[0095][0096][0097]
其中，θ为机器人的姿势角，分别指机器人中心位置在x轴和y轴方向的分速度,为机器人自旋的角速度，fi为作用在各个万向轮上的驱动力，vi为各个万向轮速度，m为机器人的质量，m为人附加给机器人的等价质量，i为机器人的转动惯量，l是机器人中心到四个全向轮的距离，r为中心到重心间距离，α是重心与中心连接线相对与姿势角的角度，为机器人重心位置在x方向与y方向的加速度,为机器人旋转的角加速度；l1＝l-rcosα，l2＝-l rsinα，l3＝-l-rcosα，l4＝-l rsinα为机器人重心处到四个轮子的距离；其中，r＝0.2 0.15sin(t)，α＝0.02t；
[0098]
当
[0099]
时，
[0100]
动力学模型即式2以表示；
[0101]
步骤2：假设在kτ时刻之后，又经过非常微小时间后的时刻为kτ

；在时刻t＝kτ

时，作用于步行训练机器人万向轮上的驱动力为f(kτ

)，在一个采样周期τ内作用于各个万向轮驱动力为不变的，则时刻t＝kτ和时刻t＝kτ

的步行训练机器人的离散型动力学模型分别为：
[0102][0103][0104]
其中f[(k-1)τ

]为时间区间[(k-1)τ

,kτ]内作用于各个万向轮的输入驱动力，f[kτ

]是经过一个采样周期τ后，下一个时间区间[kτ

,(k 1)τ]内的作用于各个万向轮的驱动力；为kg的广义逆，为时间区间[(k-1)τ

,kτ]内轮椅中心处加速度值，为时间区间[kτ

,(k 1)τ]内轮椅中心处加速度值，为t＝kτ时轮椅中心处速度值，为t＝kτ

时轮椅中心处速度值；
[0105]
步骤3：设采样周期为τ＝0.04s，把xd(t)作为步行训练机器人的目标轨迹，为了使步行训练机器人跟踪目标轨迹，基于时刻t＝kτ的万向轮驱动力为f[(k-1)τ

]，则在时刻t＝kτ

，基于数值加速度控制，假设mg和t均已知，作用在各个万向轮上的驱动力f[kτ

]的计算式如下：
[0106][0107]
其中，xd(kτ

)为t＝kτ

时刻目标轨迹xd的值，为t＝kτ

时刻目标速度的值，为t＝kτ

时刻目标加速度的值，xc(kτ)为t＝kτ时刻机器人位置xc的值，k
p
为位置偏差系数，k
p
＝diag(k
px
,k
py
,k
pθ
)，k
px
,k
py
,k
pθ
为在x轴y轴以及姿态角方向的位置偏差系数；kd为速度偏差系数kd＝diag(k
dx
,k
dy
,k
dθ
)k
dx
,k
dy
,k
dθ
为在x轴y轴以及姿态角方向的速度偏差系数；
[0108]
步骤4：由于重心偏移参数r，α时变且未知，将公式(5)中的参数r，α均设为0。公式(5)如下：
[0109][0110]
其中
[0111]
目标轨迹为xd(t)，步行训练机器人跟踪误差定义为e(t)＝xd(t)-xc(t)，步行训练机器人跟踪误差的离散模型如下：
[0112]
e[(k 1)τ]＝ae[kτ]
[0113][0114]
采样周期为τ＝0.04s，机器人的转动惯量为i＝0.7kg
·
m时，为了保证系统稳定性，基于跟踪误差的离散模型a的特征值计算，选出的k
p
和kd的最优值如下：k
p
＝diag(20，
20，20)，kd＝diag(10，10，10)，然后将其代入公式6中，完成对机器人的轨迹跟踪控制。
[0115]
实施例2；
[0116]
与实施例1不同的是：本实施例的含有重心偏移影响的步行训练机器人轨迹跟踪方法，其原理图如图3所示，还包括以下步骤:
[0117]
步骤4：对轮椅机器人的重心偏移参数r,α进行估计；
[0118]
步骤4.1：采用长短期记忆神经网络建立轮椅机器人实际对比模型，位置变化量与当前时刻驱动力关系,其原理图如图4所示；
[0119]
步骤4.1.1：使机器人空载按预定的轨迹移动，收集机器人四个轮子的驱动力记为f，机器人从t＝(k-1)τ时刻到t＝kτ时刻位置变化量数据记为δx
′c(k)；
[0120]
步骤4.1.2：对步骤4.1.1中采集的数据进行预处理；
[0121]
步骤4.1.3：设计隐藏层个数，构建长短期记忆神经网络；
[0122]
步骤4.1.4：确定最优的长短期记忆神经网络：δx
′c＝f(f)，其输入输出如图5所示。
[0123]
步骤4.2：对比模型位置与当前时刻驱动力关系如下等式；
[0124][0125]
其中，δx
′c(k)，δy
′c(k)为理想对比模型从t＝(k-1)τ时刻到t＝kτ时刻在x，y方向的位置变化量，θ
′
(k)δ为对比模型从t＝(k-1)τ时刻到t＝kτ时刻的姿态角变化量，为t＝(k-1)τ时刻对比模型在x，y方向的速度，为t＝(k-1)τ时刻对比模型的旋转角速度，，fi(k)为第i个全向轮t＝kτ时刻受到的驱动力，为矩阵mc的逆，θ
′
k-1
为t＝(k-1)τ时刻对比机器人模型的姿态角，τ为采样时间；
[0126]
当
[0127]
时，
[0128]
机器人从t＝(k-1)τ时刻到t＝kτ时刻位置变化量和驱动力之间关系以表示；
[0129]
步骤4.3：实际机器人从t＝(k-1)τ时刻到t＝kτ时刻位置变化量与当前时刻驱动力关系如下等式；
[0130][0131]
其中，其中，δxc(k)，δyc(k)为从t＝(k-1)τ时刻到t＝kτ时刻机器人在x，y方向的位置变化量，δθ(k)为从t＝(k-1)τ时刻到t＝kτ时刻机器人的姿态角变化量，为t＝(k-1)τ时刻机器人在x，y方向的速度，为t＝(k-1)τ时刻机器人的旋转角速度，θ
k-1
为t＝(k-1)τ时刻机器人的姿态角，rk,αk为t＝kτ时刻机器人的重心偏移量r,α的值；
[0132]
当
[0133][0134]
时，
[0135]
实际机器人从t＝(k-1)τ时刻到t＝kτ时刻位置变化量与当前时刻驱动力关系以表示；t-1
(k)为矩阵t(k)的逆，为矩阵mg(k)的逆；
[0136]
步骤4.4：根据式7和式8得对比模型和实际机器人之间从t＝(k-1)τ时刻到t＝kτ时刻的位置变化差与当前时刻的重心偏移量关系如下等式；
[0137][0138][0139][0140]
其中δ0x(k)为从t＝(k-1)τ时刻到t＝kτ时刻实际机器人和对比机器人在x方向位置变化量的差值，δ0y(k)为从t＝(k-1)τ时刻到t＝kτ时刻实际机器人和对比机器人在y方向位置变化量的差值，δ0θ(k)为从t＝(k-1)τ时刻到t＝kτ时刻实际机器人和对比机器
人姿态角变化量的差值。δu
cx
为从t＝(k-1)τ时刻到t＝kτ时刻机器人中心处的转矩在x方向的分量，δu
cy
为从t＝(k-1)τ时刻到t＝kτ时刻机器人中心处的转矩在y方向的分量，δu
cθ
为从t＝(k-1)τ时刻到t＝kτ时刻机器人中心处的旋转力矩；
[0141]
a1＝-r
k2
sin2(αk θ
k-1
)δu
cx
(k)-rksin(αk θ
k-1
)δu
cθ
(k) r
k2
sin(αk θ
k-1
)cos(αk θ
k-1
)δu
cy
(k)
[0142]
a2＝-r
k2
cos2(αk θ
k-1
)δu
cy
(k) rkcos(αk θ
k-1
)δu
cθ
(k) rkcos(αk θ
k-1
)sin(αk θ
k-1
)δu
cx
(k)
[0143]
a3＝-rksin(αk θ
k-1
)δu
cx
(k) 50r
k2-rkcos(αk θ
k-1
)δu
cy
(k)
[0144]
当时，
[0145]
根据式9—式11得到下式：
[0146][0147]
步骤4.5：采用牛顿迭代法求解式8—式10；
[0148]
步骤4.5.1：选定rk，αk的初始值，计算雅克比矩阵并求得雅克比矩阵的初始值f0′
(rk,αk)，f0′
(rk,αk)-1
；
[0149]
步骤4.5.2：采用r
k 1
＝r
k-f
′
(rk)-1f′
(rk)，α
k 1
＝α
k-f
′
(αk)-1f′
(αk)进行雅克比迭代；
[0150]
步骤4.5.3：当两次迭代的差值的绝对值小于10-7
时，结束迭代；
[0151]
步骤5：根据步骤4中得到的重心偏移参数估计值，对重心偏移参数进行预测，步骤如下：
[0152]
步骤5.1：采用张神经网络预测求逆方法对重心偏移参数进行预测，其原理图如图6所示，公式如下：
[0153][0154]
其中h为设计参数，为采用张神经网络估计出的t＝(k 1)τ时刻的值，kg(k)，kg(k-1)，kg(k-2)，kg(k-3)分别为t＝kτ，t＝(k-1)τ，t＝(k-2)τt＝(k-3)τ时刻kg的值。分别为t＝kτ，t＝(k-1)τ，t＝(k-2)τt＝(k-3)τ时刻的值；
[0155]
步骤5.2：由式13得到的从而求得k 1时刻重心偏移量rk 1，αk 1的估计值；
[0156][0157]
其中，为t＝(k 1)τ时刻r，α的估计值，为t＝(k 1)τ时刻l1，l2的估计值。
[0158]
步骤6：将步骤5中得到的k 1时刻的重心偏移量r
k 1
,α
k 1
带入数值加速度控制器中对机器人进行控制；
[0159][0160]
其中，为mg，t的估计值。
[0161]
步骤7：决定k
p
和kd的最优值，将目标轨迹设为xd，步行训练机器人跟踪误差定义为e(t)＝xd(t)-xc(t)，当作用在步行训练机器人万向轮上的驱动力瞬间变化时，其步行训练机器人加速度也将瞬间变化，而步行训练机器人速度和位置是不会发生瞬间变化，又由步行训练机器人的离散型动力学模型和作用于各个万向轮上的驱动力的计算式，得到步行训练机器人跟踪误差的离散模型如下：
[0162][0163]
其中e(kτ

)＝xd(kτ

)-xc(kτ)为为t＝kτ时刻机器人的速度误差，为t＝kτ时刻机器人的速度误差，为t＝kτ时刻机器人加速度误差；
[0164]
在时间区间[kτ

,(k 1)τ]内，
[0165][0166][0167]
其中为机器人速度误差，为机器人加速度误差，为t＝(k 1)τ时刻的值，为t＝kτ时刻的值，为t＝kτ时刻的值，e[(k 1)τ]为t＝(k 1)τ时刻e(t)的值，e[kτ]为t＝kτ时刻e(t)的值；
[0168]
根据式16—式18得到下式：
[0169][0170]
令以及
[0171]
得到e[(k 1)τ]＝ae[kτ]；
[0172]
对于上式的误差反馈系统，用极点配置法则能选出k
p
和kd的最优值，然后将其代入公式15中，完成对机器人的轨迹跟踪控制。。
[0173]
测试实验；
[0174]
分别对实施例1～2的对步行训练机器人轨迹跟踪方法进行仿真实验，仿真实验的轨迹如下：
[0175]
轨迹a；
[0176]
θd＝0.3t
[0177]
轨迹b；
[0178]
θd＝π/4
[0179]
仿真实验时间为20s；
[0180]
需要说明的是，图7中的点线为目标值，实线为步行训练机器人的仿真实验跟踪结果，图7(a)表示步行训练机器人在双扭线目标轨迹上的跟踪，图7(b)的纵轴表示机器人x方向位置的跟踪，图7(c)的纵轴表示机器人y方向位置的跟踪，图7(d)的纵轴表示机器人姿势角的跟踪。图8中的点线为目标值，实线为步行训练机器人的仿真实验跟踪结果，图8(a)表示步行训练机器人在双扭线目标轨迹上的跟踪，图8(b)的纵轴表示机器人x方向位置的跟踪，图8(c)的纵轴表示机器人y方向位置的跟踪，图8(d)的纵轴表示机器人姿势角的跟踪，图9(a)和(b)表示重心偏移参数t
13
＝-rsin(α θ),t
23
＝rcos(α θ)的估计值。图10中的点线为目标值，实线为步行训练机器人的仿真实验跟踪结果，图10(a)表示步行训练机器人在圆形目标轨迹上的跟踪，图10(b)的纵轴表示机器人x方向位置的跟踪，图10(c)的纵轴表示机器人y方向位置的跟踪。图11中的点线为目标值，实线为步行训练机器人的仿真实验跟踪结果，图11(a)表示步行训练机器人在双扭线目标轨迹上的跟踪，图11(b)的纵轴表示机器人x方向位置的跟踪，图11(c)的纵轴表示机器人y方向位置的跟踪；
[0181]
从图7、图8、图10和图11的结果可以看出，含有参数估计及预测的仿真实验结果，要比没有参数估计及预测的跟踪精度高。