CN115407791B - 一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法 - Google Patents
一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法 Download PDFInfo
- Publication number
- CN115407791B CN115407791B CN202210996083.9A CN202210996083A CN115407791B CN 115407791 B CN115407791 B CN 115407791B CN 202210996083 A CN202210996083 A CN 202210996083A CN 115407791 B CN115407791 B CN 115407791B
- Authority
- CN
- China
- Prior art keywords
- robot
- time
- moment
- walking training
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000012549 training Methods 0.000 title claims abstract description 84
- 230000005484 gravity Effects 0.000 title claims abstract description 82
- 238000000034 method Methods 0.000 title claims abstract description 53
- 230000001133 acceleration Effects 0.000 claims abstract description 35
- 238000013528 artificial neural network Methods 0.000 claims abstract description 20
- 238000013461 design Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 16
- 238000005070 sampling Methods 0.000 claims description 11
- 230000000052 comparative effect Effects 0.000 claims description 8
- 230000007787 long-term memory Effects 0.000 claims description 7
- 230000006403 short-term memory Effects 0.000 claims description 7
- 230000015654 memory Effects 0.000 claims description 4
- 235000004443 Ricinus communis Nutrition 0.000 claims description 3
- 238000007781 pre-processing Methods 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 abstract description 3
- 238000007430 reference method Methods 0.000 abstract description 2
- 238000004088 simulation Methods 0.000 description 12
- 238000010586 diagram Methods 0.000 description 9
- 238000004364 calculation method Methods 0.000 description 3
- 230000007547 defect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000032683 aging Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000004064 dysfunction Effects 0.000 description 1
- 230000003203 everyday effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 210000003141 lower extremity Anatomy 0.000 description 1
- 238000005312 nonlinear dynamic Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本发明设计一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法,属于机器人控制技术领域;本发明着眼于作用在各个万向轮上的驱动力和步行训练机器人位置之间的瞬间关系,设计参数估计及预测方法,实时估计并预测重心偏移量,然后采用基于数值加速度控制的步行训练机器人的轨迹跟踪控制方法;所设计的离散数值加速度控制器算法可以抵消步行训练机器人系统实时运动过程中存在的非线性时变干扰;基于模型参考法对重心偏移参数进行估计,再基于张神经网络对估计的参数进行预测,可解决估计滞后性的影响,使得位置偏差收敛到参考轨迹;在机器人运行过程中重心偏移实时变化情况下也能实现高精度的轨迹跟踪,具有良好的鲁棒性。
Description
技术领域
本发明属于机器人控制技术领域,尤其涉及一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法。
背景技术
由于老龄化人口和下肢残疾人口有一定的行走功能障碍,在平时的生活中时常发生摔倒,有研究表明,老年人每天进行行走练习不仅可以预防摔倒的发生,还能增加他们身体的稳定性。越来越多的步行训练机器人被广泛应用。全方位步行训练机器人可以在不改变姿态的情况下向任何方向移动。它非常适合在狭窄的空间中工作,已被广泛研究。
利用步行训练机器人辅助老人生活,关键的是安全性。步行训练机器人行走时为避免撞击周围的人和物,在室内全局或局部路径规划好的轨迹上,必须具备有高精度的轨迹跟踪能力。但是步行训练机器人在室内使用时,由于使用者姿态的改变,导致机器人的重心与中心位置不重合,且重心偏移是时变的。考虑重心偏移影响的轨迹跟踪控制的常用方法有PID控制,模型预测控制,自适应控制等。对于给定的目标轨迹,PID控制法在一定的地面条件,即当重心偏移一定时,通过调节其控制参数,可以达到一定的追踪精度。但是当改变轨迹,或者使用者发生变化,即重心偏移实时变化时,需要重新调节PID控制参数,否则将不能保证轨迹跟踪的精度。因此说用PID控制实现步行训练机器人轨迹跟踪时,很难实时对应各种轨迹以及适应各种不同的重心偏移变化。模型预测控制,可在满足约束条件下减小重心偏移对跟踪精度的影响,但未直面重心偏移引起的动态耦合矩阵不确定问题进行处理,所以模型预测控制不容易实现步行训练机器人的实时轨迹跟踪控制。自适应控制法不需要实时得到当前时刻重心偏移量,但其稳定性是建立在重心偏移变化比较慢的基础上,重心位置变化快会导致该控制算法不稳定。目前,全方位步行训练机器人的轨迹跟踪问题是国内外研究的难点问题。
发明内容
针对现有技术的不足,本发明设计一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法。
一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法,包括以下步骤:
步骤1:步行训练机器人全方位移动部件为四个万向轮,对步行训练机器人进行物理建模,使用者对步行训练机器人的重心偏移影响的运动学模型和动力学模型分别为:
其中,θ为机器人的姿势角,分别指机器人中心位置在x轴和y轴方向的分速度,/>为机器人自旋的角速度,fi为作用在各个万向轮上的驱动力,vi为各个万向轮速度,M为机器人的质量,m为人附加给机器人的等价质量,I为机器人的转动惯量,l是机器人中心到四个全向轮的距离,r为中心到重心间距离,α是重心与中心连接线相对与姿势角的角度,/>为机器人中心位置在x方向与y方向的加速度,/>为机器人旋转的角加速度;l1=l-rcosα,l2=-l+rsinα,l3=-l-rcosα,l4=-l+rsinα为机器人重心处到四个轮子的距离;
当
时,
动力学模型即式2以表示;
步骤2:假设在kτ时刻之后,又经过非常微小时间后的时刻为kτ+;在时刻t=kτ+时,作用于步行训练机器人万向轮上的驱动力为F(kτ+),在一个采样周期τ内作用于各个万向轮驱动力为不变的,则时刻t=kτ和时刻t=kτ+的步行训练机器人的离散型动力学模型分别为:
其中F[(k-1)τ+]为时间区间[(k-1)τ+,kτ]内作用于各个万向轮的输入驱动力,F[kτ+]是经过一个采样周期τ后,下一个时间区间[kτ+,(k+1)τ]内的作用于各个万向轮的驱动力;为KG的广义逆,/>为时间区间[(k-1)τ+,kτ]内轮椅中心处加速度值,/>为时间区间[kτ+,(k+1)τ]内轮椅中心处加速度值,/>为t=kτ时轮椅中心处速度值,为t=kτ+时轮椅中心处速度值;
步骤3:把Xd(t)作为步行训练机器人的目标轨迹,为了使步行训练机器人跟踪目标轨迹,基于时刻t=kτ的万向轮驱动力为F[(k-1)τ+],则在时刻t=kτ+,基于数值加速度控制,假设MG和T均已知,作用在各个万向轮上的驱动力F[kτ+]的计算式如下:
其中,Xd(kτ+)为t=kτ+时刻目标轨迹Xd的值,为t=kτ+时刻目标速度/>的值,/>为t=kτ+时刻目标加速度/>的值,XC(kτ)为t=kτ时刻机器人位置XC的值,KP为位置偏差系数,KP=diag(kPx,kPy,kPθ),kPx,kPy,kPθ为在x轴y轴以及姿态角方向的位置偏差系数;KD为速度偏差系数KD=diag(kDx,kDy,kDθ)kDx,kDy,kDθ为在x轴y轴以及姿态角方向的速度偏差系数;
步骤4:对轮椅机器人的重心偏移参数r,α进行估计;
步骤4.1:采用长短期记忆神经网络建立轮椅机器人实际对比模型位置变化量与当前时刻驱动力关系;
步骤4.1.1:使机器人空载按预定的轨迹移动,收集机器人四个轮子的驱动力记为F,机器人从t=(k-1)τ时刻到t=kτ时刻位置变化量数据记为ΔX′C(k);
步骤4.1.2:对步骤4.1.1中采集的数据进行预处理;
步骤4.1.3:设计隐藏层个数,构建长短期记忆神经网络;
步骤4.1.4:确定最优的长短期记忆神经网络:ΔX′C=f(F);
步骤4.2:理想对比模型位置变化量与当前时刻驱动力关系如下等式;
其中,Δ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)τ时刻对比机器人模型的姿态角,τ为采样时间;
当
时,
机器人从t=(k-1)τ时刻到t=kτ时刻位置变化量和驱动力之间关系以表示;
步骤4.3:实际机器人从t=(k-1)τ时刻到t=kτ时刻位置变化量与当前时刻驱动力关系如下等式;
其中,Δ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,α的值;
当
时,
实际机器人从t=(k-1)τ时刻到t=kτ时刻位置变化量与当前时刻驱动力关系以表示;T-1(k)为矩阵T(k)的逆,为矩阵MG(k)的逆;
步骤4.4:根据式6和式7得对比模型和实际机器人之间从t=(k-1)τ时刻到t=kτ时刻的位置变化差与当前时刻的重心偏移量关系如下等式;
其中:其中Δ0x(k)为从t=(k-1)τ时刻到t=kτ时刻实际机器人和对比机器人在x方向位置变化量的差值,Δ0y(k)为从t=(k-1)τ时刻到t=kτ时刻实际机器人和对比机器人在y方向位置变化量的差值,Δ0θ(k)为从t=(k-1)τ时刻到t=kτ时刻实际机器人和对比机器人姿态角变化量的差值。ΔuCx为从t=(k-1)τ时刻到t=kτ时刻机器人中心处的转矩在x方向的分量,ΔuCy为从t=(k-1)τ时刻到t=kτ时刻机器人中心处的转矩在y方向的分量,ΔuCθ为从t=(k-1)τ时刻到t=kτ时刻机器人中心处的旋转力矩;
A1=-rk 2sin2(αk+θk-1)ΔuCx(k)-rksin(αk+θk-1)ΔuCθ(k)+rk 2sin(αk+θk-1)cos(αk+θk-1)ΔuCy(k)
A2=-rk 2cos2(αk+θk-1)ΔuCy(k)+rkcos(αk+θk-1)ΔuCθ(k)+rkcos(αk+θk-1)sin(αk+θk-1)ΔuCx(k)
A3=-rksin(αk+θk-1)ΔuCx(k)+mrk 2-rkcos(αk+θk-1)ΔuCy(k)
当,时,
根据式8—式10得到下式:
步骤4.5:采用牛顿迭代法求解式8—式10;
步骤4.5.1:选定rk,αk的初始值,计算雅克比矩阵并求得雅克比矩阵的初始值F0′(rk,αk),F0′(rk,αk)-1;
步骤4.5.2:采用rk+1=rk-F′(rk)-1F′(rk),αk+1=αk-F′(αk)-1F′(αk)进行雅克比迭代;
步骤4.5.3:当两次迭代的差值的绝对值小于10-7时,结束迭代;
步骤5:根据步骤4中得到的重心偏移参数估计值,对重心偏移参数进行预测,步骤如下:
步骤5.1:采用张神经网络预测求逆方法对重心偏移参数进行预测,公式如下:
其中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)τ时刻/>的值;
步骤5.2:由式12得到的从而求得k+1时刻重心偏移量rk+1,αk+1的估计值;
其中,为t=(k+1)τ时刻r,α的估计值,/>为t=(k+1)τ时刻l1,l2的估计值;
步骤6:将步骤5中得到的k+1时刻的重心偏移量rk+1,αk+1带入数值加速度控制器中对机器人进行控制;
其中,为MG,T的估计值;
步骤7:决定KP和KD的最优值,将目标轨迹设为Xd,步行训练机器人跟踪误差定义为e(t)=Xd(t)-XC(t),当作用在步行训练机器人万向轮上的驱动力瞬间变化时,其步行训练机器人加速度也将瞬间变化,而步行训练机器人速度和位置是不会发生瞬间变化,又由步行训练机器人的离散型动力学模型和作用于各个万向轮上的驱动力的计算式,得到步行训练机器人跟踪误差的离散模型如下:
其中e(kτ+)=Xd(kτ+)-XC(kτ)为为t=kτ时刻机器人的速度误差,为t=kτ时刻机器人的速度误差,/>为t=kτ时刻机器人加速度误差;
在时间区间[kτ+,(k+1)τ]内,
其中为机器人速度误差,/>为机器人加速度误差,/>为t=(k+1)τ时刻/>的值,/>为t=kτ时刻/>的值,/>为t=kτ时刻/>的值,e[(k+1)τ]为t=(k+1)τ时刻e(t)的值,e[kτ]为t=kτ时刻e(t)的值;
根据式15—式17得到下式:
令
以及
得到E[(k+1)τ]=AE[kτ];
对于上式的误差反馈系统,用极点配置法则能选出KP和KD的最优值,然后将其代入公式14中,完成对机器人的轨迹跟踪控制。
本发明有益技术效果:
本发明着眼于作用在各个万向轮上的驱动力和步行训练机器人位置之间的瞬间关系,设计参数估计及预测方法,实时估计并预测重心偏移量,然后采用基于数值加速度控制的步行训练机器人的轨迹跟踪控制方法。所设计的离散数值加速度控制器算法可以抵消步行训练机器人系统实时运动过程中存在的非线性时变干扰。基于模型参考法对重心偏移参数进行估计,再基于张神经网络对估计的参数进行预测,可解决估计滞后性的影响,使得位置偏差收敛到参考轨迹。
本发明的重心参数预测方法,使步行训练机器人的轨迹跟踪精度得到进一步有效提高。本发明易于计算机实时控制的实现,在机器人运行过程中重心偏移实时变化情况下也能实现高精度的轨迹跟踪,具有良好的鲁棒性。
附图说明
图1为本发明的步行训练机器人的结构示意图。
图2为本发明中步行训练机器人万向轮的坐标配置图。
图3为本发明中步行训练机器人控制系统的框图。
图4为本发明中长短期记忆神经网络建立对比模型的原理图。
图5为本发明中长短期记忆神经网络输入输出数据图。
图6为本发明中参数预测原理图。
图7为本发明中实施例1轨迹A的仿真实验结果图。
图8为本发明中实施例2轨迹A的仿真实验结果图。
图9为本发明中实施例2轨迹A的参数估计及预测结果图。
图10为本发明中实施例1轨迹B的仿真实验结果图。
图11为本发明中实施例2轨迹B的仿真实验结果图。
具体实施方式
下面将结合具体实施方式和说明书附图对本发明及其有益效果作进一步详细说明。
本发明针对现有技术的不足,提供一种含有重心偏移影响的步行训练机器人轨迹跟踪方法,该方法首先基于牛顿-欧拉刚体学原理建立考虑重心偏移的动力学模型并基于长短期记忆神经网络训练构建其对比模型。其次基于模型对比法实时估计重心偏移参数,再基于张神经网络对估计的重心偏移参数进行预测以减小估计过程引起的滞后,最后基于非线性动态反馈解耦法设计数值加速度控制实现。另外,该方法的数值加速度控制器能很好地处理步行训练机器人重心实时变化的影响,并且由构成的跟踪误差状态反馈形式,用极点配置法可以选出数值控制器中的最优调节参数,轨迹跟踪精度将得到进一步有效提高。
本实施例提供一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法,包括以下步骤:
步骤1:步行训练机器人全方位移动部件为四个万向轮,当步行训练机器人的质量M=80kg,步行训练机器人的转动惯量I=0.7kg·m,驱动力相对机器人中心位置的力臂长度l=0.6m时,图1所示的步行训练机器人,按图2所示的万向轮的坐标配置,其运动学模型和动力学模型以及重心偏移模型如下:
其中,θ为机器人的姿势角,分别指机器人中心位置在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;
当
时,
动力学模型即式2以表示;
步骤2:假设在kτ时刻之后,又经过非常微小时间后的时刻为kτ+;在时刻t=kτ+时,作用于步行训练机器人万向轮上的驱动力为F(kτ+),在一个采样周期τ内作用于各个万向轮驱动力为不变的,则时刻t=kτ和时刻t=kτ+的步行训练机器人的离散型动力学模型分别为:
其中F[(k-1)τ+]为时间区间[(k-1)τ+,kτ]内作用于各个万向轮的输入驱动力,F[kτ+]是经过一个采样周期τ后,下一个时间区间[kτ+,(k+1)τ]内的作用于各个万向轮的驱动力;为KG的广义逆,/>为时间区间[(k-1)τ+,kτ]内轮椅中心处加速度值,/>为时间区间[kτ+,(k+1)τ]内轮椅中心处加速度值,/>为t=kτ时轮椅中心处速度值,为t=kτ+时轮椅中心处速度值;
步骤3:设采样周期为τ=0.04s,把Xd(t)作为步行训练机器人的目标轨迹,为了使步行训练机器人跟踪目标轨迹,基于时刻t=kτ的万向轮驱动力为F[(k-1)τ+],则在时刻t=kτ+,基于数值加速度控制,假设MG和T均已知,作用在各个万向轮上的驱动力F[kτ+]的计算式如下:
其中,Xd(kτ+)为t=kτ+时刻目标轨迹Xd的值,为t=kτ+时刻目标速度/>的值,/>为t=kτ+时刻目标加速度/>的值,XC(kτ)为t=kτ时刻机器人位置XC的值,KP为位置偏差系数,KP=diag(kPx,kPy,kPθ),kPx,kPy,kPθ为在x轴y轴以及姿态角方向的位置偏差系数;KD为速度偏差系数KD=diag(kDx,kDy,kDθ)kDx,kDy,kDθ为在x轴y轴以及姿态角方向的速度偏差系数;
步骤4:由于重心偏移参数r,α时变且未知,将公式(5)中的参数r,α均设为0。公式(5)如下:
其中
目标轨迹为Xd(t),步行训练机器人跟踪误差定义为e(t)=Xd(t)-XC(t),步行训练机器人跟踪误差的离散模型如下:
E[(k+1)τ]=AE[kτ]
采样周期为τ=0.04s,机器人的转动惯量为I=0.7kg·m时,为了保证系统稳定性,基于跟踪误差的离散模型A的特征值计算,选出的KP和KD的最优值如下:KP=diag(20,20,20),KD=diag(10,10,10),然后将其代入公式6中,完成对机器人的轨迹跟踪控制。
实施例2;
与实施例1不同的是:本实施例的含有重心偏移影响的步行训练机器人轨迹跟踪方法,其原理图如图3所示,还包括以下步骤:
步骤4:对轮椅机器人的重心偏移参数r,α进行估计;
步骤4.1:采用长短期记忆神经网络建立轮椅机器人实际对比模型,位置变化量与当前时刻驱动力关系,其原理图如图4所示;
步骤4.1.1:使机器人空载按预定的轨迹移动,收集机器人四个轮子的驱动力记为F,机器人从t=(k-1)τ时刻到t=kτ时刻位置变化量数据记为ΔX′C(k);
步骤4.1.2:对步骤4.1.1中采集的数据进行预处理;
步骤4.1.3:设计隐藏层个数,构建长短期记忆神经网络;
步骤4.1.4:确定最优的长短期记忆神经网络:ΔX′C=f(F),其输入输出如图5所示。
步骤4.2:对比模型位置与当前时刻驱动力关系如下等式;
其中,Δ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)τ时刻对比机器人模型的姿态角,τ为采样时间;
当
时,
机器人从t=(k-1)τ时刻到t=kτ时刻位置变化量和驱动力之间关系以表示;
步骤4.3:实际机器人从t=(k-1)τ时刻到t=kτ时刻位置变化量与当前时刻驱动力关系如下等式;
其中,其中,Δ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,α的值;
当
时,
实际机器人从t=(k-1)τ时刻到t=kτ时刻位置变化量与当前时刻驱动力关系以表示;T-1(k)为矩阵T(k)的逆,为矩阵MG(k)的逆;
步骤4.4:根据式7和式8得对比模型和实际机器人之间从t=(k-1)τ时刻到t=kτ时刻的位置变化差与当前时刻的重心偏移量关系如下等式;
/>
其中Δ0x(k)为从t=(k-1)τ时刻到t=kτ时刻实际机器人和对比机器人在x方向位置变化量的差值,Δ0y(k)为从t=(k-1)τ时刻到t=kτ时刻实际机器人和对比机器人在y方向位置变化量的差值,Δ0θ(k)为从t=(k-1)τ时刻到t=kτ时刻实际机器人和对比机器人姿态角变化量的差值。ΔuCx为从t=(k-1)τ时刻到t=kτ时刻机器人中心处的转矩在x方向的分量,ΔuCy为从t=(k-1)τ时刻到t=kτ时刻机器人中心处的转矩在y方向的分量,ΔuCθ为从t=(k-1)τ时刻到t=kτ时刻机器人中心处的旋转力矩;
A1=-rk 2sin2(αk+θk-1)ΔuCx(k)-rksin(αk+θk-1)ΔuCθ(k)+rk 2sin(αk+θk-1)cos(αk+θk-1)ΔuCy(k)
A2=-rk 2cos2(αk+θk-1)ΔuCy(k)+rkcos(αk+θk-1)ΔuCθ(k)+rkcos(αk+θk-1)sin(αk+θk-1)ΔuCx(k)
A3=-rksin(αk+θk-1)ΔuCx(k)+50rk 2-rkcos(αk+θk-1)ΔuCy(k)
当时,
根据式9—式11得到下式:
步骤4.5:采用牛顿迭代法求解式8—式10;
步骤4.5.1:选定rk,αk的初始值,计算雅克比矩阵并求得雅克比矩阵的初始值F0′(rk,αk),F0′(rk,αk)-1;
步骤4.5.2:采用rk+1=rk-F′(rk)-1F′(rk),αk+1=αk-F′(αk)-1F′(αk)进行雅克比迭代;
步骤4.5.3:当两次迭代的差值的绝对值小于10-7时,结束迭代;
步骤5:根据步骤4中得到的重心偏移参数估计值,对重心偏移参数进行预测,步骤如下:
步骤5.1:采用张神经网络预测求逆方法对重心偏移参数进行预测,其原理图如图6所示,公式如下:
其中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)τ时刻/>的值;
步骤5.2:由式13得到的从而求得k+1时刻重心偏移量rk+1,αk+1的估计值;
其中,为t=(k+1)τ时刻r,α的估计值,/>为t=(k+1)τ时刻l1,l2的估计值。
步骤6:将步骤5中得到的k+1时刻的重心偏移量rk+1,αk+1带入数值加速度控制器中对机器人进行控制;
其中,为MG,T的估计值。
步骤7:决定KP和KD的最优值,将目标轨迹设为Xd,步行训练机器人跟踪误差定义为e(t)=Xd(t)-XC(t),当作用在步行训练机器人万向轮上的驱动力瞬间变化时,其步行训练机器人加速度也将瞬间变化,而步行训练机器人速度和位置是不会发生瞬间变化,又由步行训练机器人的离散型动力学模型和作用于各个万向轮上的驱动力的计算式,得到步行训练机器人跟踪误差的离散模型如下:
其中e(kτ+)=Xd(kτ+)-XC(kτ)为为t=kτ时刻机器人的速度误差,为t=kτ时刻机器人的速度误差,/>为t=kτ时刻机器人加速度误差;
在时间区间[kτ+,(k+1)τ]内,
其中为机器人速度误差,/>为机器人加速度误差,/>为t=(k+1)τ时刻/>的值,/>为t=kτ时刻/>的值,/>为t=kτ时刻/>的值,e[(k+1)τ]为t=(k+1)τ时刻e(t)的值,e[kτ]为t=kτ时刻e(t)的值;
根据式16—式18得到下式:
令以及/>
得到E[(k+1)τ]=AE[kτ];
对于上式的误差反馈系统,用极点配置法则能选出KP和KD的最优值,然后将其代入公式15中,完成对机器人的轨迹跟踪控制。。
测试实验;
分别对实施例1~2的对步行训练机器人轨迹跟踪方法进行仿真实验,仿真实验的轨迹如下:
轨迹A;
θd=0.3t
轨迹B;
θd=π/4
仿真实验时间为20s;
需要说明的是,图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)表示重心偏移参数t13=-rsin(α+θ),t23=rcos(α+θ)的估计值。图10中的点线为目标值,实线为步行训练机器人的仿真实验跟踪结果,图10(a)表示步行训练机器人在圆形目标轨迹上的跟踪,图10(b)的纵轴表示机器人x方向位置的跟踪,图10(c)的纵轴表示机器人y方向位置的跟踪。图11中的点线为目标值,实线为步行训练机器人的仿真实验跟踪结果,图11(a)表示步行训练机器人在双扭线目标轨迹上的跟踪,图11(b)的纵轴表示机器人x方向位置的跟踪,图11(c)的纵轴表示机器人y方向位置的跟踪;
从图7、图8、图10和图11的结果可以看出,含有参数估计及预测的仿真实验结果,要比没有参数估计及预测的跟踪精度高。
Claims (5)
1.一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法,其特征在于,包括以下步骤:
步骤1:步行训练机器人全方位移动部件为四个万向轮,对步行训练机器人进行物理建模;
步骤2:假设在kτ时刻之后,又经过非常微小时间后的时刻为kτ+;在时刻t=kτ+时,作用于步行训练机器人万向轮上的驱动力为F(kτ+),在一个采样周期τ内作用于各个万向轮驱动力为不变的,则得到时刻t=kτ和时刻t=kτ+的步行训练机器人的离散型动力学模型;
步骤3:把Xd(t)作为步行训练机器人的目标轨迹,为了使步行训练机器人跟踪目标轨迹,基于时刻t=kτ的万向轮驱动力为F[(k-1)τ+],则在时刻t=kτ+,基于数值加速度控制,得到作用在各个万向轮上的驱动力F[kτ+];
步骤4:对轮椅机器人的重心偏移参数r,α进行估计;
步骤5:根据步骤4中得到的重心偏移参数估计值,对重心偏移参数进行预测;
步骤6:将步骤5中得到的k+1时刻的重心偏移量rk+1,αk+1带入数值加速度控制器中对机器人进行控制;
步骤7:决定KP和KD的最优值,将目标轨迹设为Xd,步行训练机器人跟踪误差定义为e(t)=Xd(t)-XC(t),当作用在步行训练机器人万向轮上的驱动力瞬间变化时,其步行训练机器人加速度也将瞬间变化,而步行训练机器人速度和位置是不会发生瞬间变化,又由步行训练机器人的离散型动力学模型和作用于各个万向轮上的驱动力的计算式,得到步行训练机器人跟踪误差的离散模型;用极点配置法则能选出KP和KD的最优值,然后将其代入步骤3F[kτ+]式子中,完成对机器人的轨迹跟踪控制;
步骤1使用者对步行训练机器人的重心偏移影响的运动学模型和动力学模型分别为:
其中,θ为机器人的姿势角,分别指机器人中心位置在x轴和y轴方向的分速度,/>为机器人自旋的角速度,fi为作用在各个万向轮上的驱动力,vi为各个万向轮速度,M为机器人的质量,m为人附加给机器人的等价质量,I为机器人的转动惯量,l是机器人中心到四个全向轮的距离,r为中心到重心间距离,α是重心与中心连接线相对与姿势角的角度,/>为机器人中心位置在x方向与y方向的加速度,/>为机器人旋转的角加速度;l1=l-rcosα,l2=-l+rsinα,l3=-l-rcosα,l4=-l+rsinα为机器人重心处到四个轮子的距离;
当
时,
动力学模型即式2以表示;
步骤2时刻t=kτ和时刻t=kτ+的步行训练机器人的离散型动力学模型分别为:
其中F[(k-1)τ+]为时间区间[(k-1)τ+,kτ]内作用于各个万向轮的输入驱动力,F[kτ+]是经过一个采样周期τ后,下一个时间区间[kτ+,(k+1)τ]内的作用于各个万向轮的驱动力;为KG的广义逆,/>为时间区间[(k-1)τ+,kτ]内轮椅中心处加速度值,/>为时间区间[kτ+,(k+1)τ]内轮椅中心处加速度值,/>为t=kτ时轮椅中心处速度值,为t=kτ+时轮椅中心处速度值;
步骤3假设MG和T均已知,作用在各个万向轮上的驱动力F[kτ+]的计算式如下:
其中,Xd(kτ+)为t=kτ+时刻目标轨迹Xd的值,为t=kτ+时刻目标速度/>的值,为t=kτ+时刻目标加速度/>的值,XC(kτ)为t=kτ时刻机器人位置XC的值,KP为位置偏差系数,KP=diag(kPx,kPy,kPθ),kPx,kPy,kPθ为在x轴y轴以及姿态角方向的位置偏差系数;KD为速度偏差系数KD=diag(kDx,kDy,kDθ)kDx,kDy,kDθ为在x轴y轴以及姿态角方向的速度偏差系数。
2.根据权利要求1所述的一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法,其特征在于,步骤4具体为:
步骤4.1:采用长短期记忆神经网络建立轮椅机器人实际对比模型位置变化量与当前时刻驱动力关系;
步骤4.1.1:使机器人空载按预定的轨迹移动,收集机器人四个轮子的驱动力记为F,机器人从t=(k-1)τ时刻到t=kτ时刻位置变化量数据记为ΔX′C(k);
步骤4.1.2:对步骤4.1.1中采集的数据进行预处理;
步骤4.1.3:设计隐藏层个数,构建长短期记忆神经网络;
步骤4.1.4:确定最优的长短期记忆神经网络:ΔX′C=f(F);
步骤4.2:理想对比模型位置变化量与当前时刻驱动力关系如下等式;
其中,Δ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)τ时刻对比机器人模型的姿态角,τ为采样时间;
当
时,
机器人从t=(k-1)τ时刻到t=kτ时刻位置变化量和驱动力之间关系以表示;
步骤4.3:实际机器人从t=(k-1)τ时刻到t=kτ时刻位置变化量与当前时刻驱动力关系如下等式;
其中,Δ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,α的值;
当
时,
实际机器人从t=(k-1)τ时刻到t=kτ时刻位置变化量与当前时刻驱动力关系以表示;T-1(k)为矩阵T(k)的逆,为矩阵MG(k)的逆;
步骤4.4:根据式6和式7得对比模型和实际机器人之间从t=(k-1)τ时刻到t=kτ时刻的位置变化差与当前时刻的重心偏移量关系如下等式;
其中Δ0x(k)为从t=(k-1)τ时刻到t=kτ时刻实际机器人和对比机器人在x方向位置变化量的差值,Δ0y(k)为从t=(k-1)τ时刻到t=kτ时刻实际机器人和对比机器人在y方向位置变化量的差值,Δ0θ(k)为从t=(k-1)τ时刻到t=kτ时刻实际机器人和对比机器人姿态角变化量的差值;ΔuCx为从t=(k-1)τ时刻到t=kτ时刻机器人中心处的转矩在x方向的分量,ΔuCy为从t=(k-1)τ时刻到t=kτ时刻机器人中心处的转矩在y方向的分量,ΔuCθ为从t=(k-1)τ时刻到t=kτ时刻机器人中心处的旋转力矩;
当,时,
根据式8—式10得到下式:
步骤4.5:采用牛顿迭代法求解式8—式10;
步骤4.5.1:选定rk,αk的初始值,计算雅克比矩阵并求得雅克比矩阵的初始值F0′(rk,αk),F′0(rk,αk)-1;
步骤4.5.2:采用rk+1=rk-F′(rk)-1F′(rk),αk+1=αk-F′(αk)-1F′(αk)进行雅克比迭代;
步骤4.5.3:当两次迭代的差值的绝对值小于10-7时,结束迭代。
3.根据权利要求1所述的一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法,其特征在于,步骤5具体为:
步骤5.1:采用张神经网络预测求逆方法对重心偏移参数进行预测,公式如下:
其中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)τ时刻/>的值;
步骤5.2:由式12得到的从而求得k+1时刻重心偏移量rk+1,αk+1的估计值;
其中,为t=(k+1)τ时刻r,α的估计值,/>为t=(k+1)τ时刻l1,l2的估计值。
4.根据权利要求1所述的一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法,其特征在于,步骤6将步骤5中得到的k+1时刻的重心偏移量rk+1,αk+1带入数值加速度控制器中对机器人进行控制;
其中,为MG,T的估计值。
5.根据权利要求1所述的一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法,其特征在于,步骤7得到步行训练机器人跟踪误差的离散模型如下:
其中e(kτ+)=Xd(kτ+)-XC(kτ)为为t=kτ时刻机器人的速度误差,为t=kτ时刻机器人的速度误差,/>为t=kτ时刻机器人加速度误差;
在时间区间[kτ+,(k+1)τ]内,
其中为机器人速度误差,/>为机器人加速度误差,/>为t=(k+1)τ时刻/>的值,/>为t=kτ时刻/>的值,/>为t=kτ时刻的值,e[(k+1)τ]为t=(k+1)τ时刻e(t)的值,e[kτ]为t=kτ时刻e(t)的值;
根据式15—式17得到下式:
令
以及
得到E[(k+1)τ]=AE[kτ];
对于上式的误差反馈系统,用极点配置法则能选出KP和KD的最优值,然后将其代入公式14中,完成对机器人的轨迹跟踪控制。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210996083.9A CN115407791B (zh) | 2022-08-19 | 2022-08-19 | 一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210996083.9A CN115407791B (zh) | 2022-08-19 | 2022-08-19 | 一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115407791A CN115407791A (zh) | 2022-11-29 |
CN115407791B true CN115407791B (zh) | 2023-12-12 |
Family
ID=84159318
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210996083.9A Active CN115407791B (zh) | 2022-08-19 | 2022-08-19 | 一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115407791B (zh) |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2013116530A (ja) * | 2011-12-02 | 2013-06-13 | Honda Motor Co Ltd | 脚式移動ロボットの歩容生成装置及びロボットの動作目標生成装置 |
JP2013116528A (ja) * | 2011-12-02 | 2013-06-13 | Honda Motor Co Ltd | 脚式移動ロボットの歩容生成装置 |
CN104375503A (zh) * | 2013-11-20 | 2015-02-25 | 沈阳工业大学 | 一种独立于康复者质量的轮式康复机器人的控制方法 |
CN104635738A (zh) * | 2014-11-21 | 2015-05-20 | 沈阳工业大学 | 不确定康复步行训练机器人的精确轨迹跟踪最优控制方法 |
CN106794576A (zh) * | 2014-04-17 | 2017-05-31 | 软银机器人欧洲公司 | 基于线性预测位置和速度控制器的全向轮式类人机器人 |
CN107656500A (zh) * | 2017-10-16 | 2018-02-02 | 广东智爱机器人科技有限公司 | 基于数值加速度控制的全方位移动机器人轨迹跟踪方法 |
CN107985318A (zh) * | 2017-11-29 | 2018-05-04 | 吉林大学 | 轮式装载机重心纵向位置和重心高度动态估计方法 |
CN111913490A (zh) * | 2020-08-18 | 2020-11-10 | 山东大学 | 基于落足调整的四足机器人动步态稳定控制方法及系统 |
CN112243401A (zh) * | 2018-06-05 | 2021-01-19 | 索尼公司 | 信息处理装置、信息处理方法以及程序 |
CN113176733A (zh) * | 2021-04-27 | 2021-07-27 | 广东工业大学 | 一种基于切换控制的自主车辆路径跟踪与稳定性控制方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11772264B2 (en) * | 2020-11-18 | 2023-10-03 | Dibi (Chongqing) Intelligent Technology Research Institute Co., Ltd. | Neural network adaptive tracking control method for joint robots |
-
2022
- 2022-08-19 CN CN202210996083.9A patent/CN115407791B/zh active Active
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2013116530A (ja) * | 2011-12-02 | 2013-06-13 | Honda Motor Co Ltd | 脚式移動ロボットの歩容生成装置及びロボットの動作目標生成装置 |
JP2013116528A (ja) * | 2011-12-02 | 2013-06-13 | Honda Motor Co Ltd | 脚式移動ロボットの歩容生成装置 |
CN104375503A (zh) * | 2013-11-20 | 2015-02-25 | 沈阳工业大学 | 一种独立于康复者质量的轮式康复机器人的控制方法 |
CN106794576A (zh) * | 2014-04-17 | 2017-05-31 | 软银机器人欧洲公司 | 基于线性预测位置和速度控制器的全向轮式类人机器人 |
CN104635738A (zh) * | 2014-11-21 | 2015-05-20 | 沈阳工业大学 | 不确定康复步行训练机器人的精确轨迹跟踪最优控制方法 |
CN107656500A (zh) * | 2017-10-16 | 2018-02-02 | 广东智爱机器人科技有限公司 | 基于数值加速度控制的全方位移动机器人轨迹跟踪方法 |
CN107985318A (zh) * | 2017-11-29 | 2018-05-04 | 吉林大学 | 轮式装载机重心纵向位置和重心高度动态估计方法 |
CN112243401A (zh) * | 2018-06-05 | 2021-01-19 | 索尼公司 | 信息处理装置、信息处理方法以及程序 |
CN111913490A (zh) * | 2020-08-18 | 2020-11-10 | 山东大学 | 基于落足调整的四足机器人动步态稳定控制方法及系统 |
CN113176733A (zh) * | 2021-04-27 | 2021-07-27 | 广东工业大学 | 一种基于切换控制的自主车辆路径跟踪与稳定性控制方法 |
Non-Patent Citations (3)
Title |
---|
A Barycenter Control Method for the Bioinspired Forest Chassis Robot on Slope;Tingting Sui 等;《Journal of Robotics》;第1-15页 * |
Zhang neural network for online solution of time-varying convex quadratic program subject to time-varying linear-equality constraints;Yunong Zhang 等;《Physics Letters A》;第373卷(第19期);第1639-1643页 * |
鲁棒轨迹跟踪方法对全方位康复机器人重心偏移的应用;姜莹;现代制造技术与装备;总第261期(第08期);第31, 35页 * |
Also Published As
Publication number | Publication date |
---|---|
CN115407791A (zh) | 2022-11-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109732605B (zh) | 一种机器人关节摩擦力矩的补偿方法及系统 | |
Dian et al. | Modeling and trajectory tracking control for magnetic wheeled mobile robots based on improved dual-heuristic dynamic programming | |
Chen et al. | Gait prediction and variable admittance control for lower limb exoskeleton with measurement delay and extended-state-observer | |
CN111290272B (zh) | 基于多足机器人的姿态平稳性调整方法 | |
CN112417755A (zh) | 一种主从式手术机器人轨迹预测控制方法 | |
CN109605377A (zh) | 一种基于强化学习的机器人关节运动控制方法及系统 | |
CN115256395B (zh) | 基于控制障碍函数的模型不确定机器人安全控制方法 | |
Qin et al. | An improved real-time slip model identification method for autonomous tracked vehicles using forward trajectory prediction compensation | |
Lin et al. | Adaptive critic anti-slip control of wheeled autonomous robot | |
Chen et al. | Ball-on-plate motion planning for six-parallel-legged robots walking on irregular terrains using pure haptic information | |
Basiuk et al. | Mobile Robot Position Determining Using Odometry Method | |
CN114942593A (zh) | 一种基于干扰观测器补偿的机械臂自适应滑模控制方法 | |
Liang et al. | A novel impedance control method of rubber unstacking robot dealing with unpredictable and time-variable adhesion force | |
Tan et al. | A cerebellum-inspired network model and learning approaches for solving kinematic tracking control of redundant manipulators | |
CN109249393B (zh) | 一种基于经验控制的多参数机器人实时行为校正方法 | |
CN115407791B (zh) | 一种考虑重心偏移影响的步行训练机器人轨迹跟踪方法 | |
CN113805610B (zh) | 一种轨迹跟踪控制方法、装置及存储介质 | |
CN109648566B (zh) | 电机参数未知的全方位移动机器人的轨迹跟踪控制方法 | |
CN116872197A (zh) | 单杆机械臂的自适应神经网络反演控制方法及其控制系统 | |
CN113515044B (zh) | 一种轮式移动机器人的基于学习预测跟踪控制方法及装置 | |
Zhao et al. | Robotic peg-in-hole assembly based on reversible dynamic movement primitives and trajectory optimization | |
CN111158238B (zh) | 一种基于粒子群算法的力反馈设备动力学参数估计算法 | |
Du et al. | Research on control algorithm for laser guided AGV based on proximal policy | |
Yin et al. | A positioning algorithm based on improved adaptive unscented Kalman filter | |
Zhang et al. | Human-like motion of robotic arm based on dynamic movement primitives and deep neural network |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |