[发明专利]康复步行训练机器人的轨迹跟踪误差约束安全控制方法

摘要

康复步行训练机器人的轨迹跟踪误差约束安全控制方法属于轮式康复机器人的控制领域，尤其涉及一种康复步行训练机器人的轨迹跟踪误差约束安全控制方法。本发明提供一种使康复步行训练机器人在运动过程中，暂态阶段和稳态阶段轨迹跟踪误差同时受约束的康复步行训练机器人的轨迹跟踪误差约束安全控制方法。本发明基于康复步行训练机器人的动力学模型，设计非线性控制器，建立x,y轴和旋转角三个运动方向的轨迹跟踪误差状态方程；基于Lyapunov稳定理论，构建各轨迹跟踪误差系统渐近稳定的约束条件，获得非线性控制器参数矩阵的求解方法，将康复步行训练机器人暂态阶段和稳态阶段的实际运动轨迹控制在指定范围内，保障康复者安全。

主权项

1.康复步行训练机器人的轨迹跟踪误差约束安全控制方法，其特征在于包括以下步骤：步骤1)基于康复步行训练机器人的动力学模型，根据运动速度和运动位置跟踪误差，建立x,y轴和旋转角三个运动方向的轨迹跟踪误差状态方程，系统的运动学模型如下：其中X(t)为康复步行训练机器人的实际行走轨迹，u(t)表示控制输入力，M表示康复步行训练机器人的质量，m表示康复者的质量，I₀表示转动惯量，为系数矩阵；θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角，θ＝θ₁，根据康复步行机器人结构，θ₃＝θ+π，l_i表示系统重心到每个轮子中心的距离，r₀表示中心到重心的距离，φ_i表示x′轴和每个轮子对应的l_i之间的夹角，i＝1,2,3,4；f₁,f₂,f₃,f₄分别表示四个驱动轮的控制输入力，λ₁,λ₂,λ₃,λ₄分别表示重心到各轮子的距离，φ₁,φ₂,φ₃,φ₄分别表示水平坐标轴和重心到各轮子中心连线的夹角；步骤2)基于康复步行训练机器人的动力学模型，根据运动速度和运动位置跟踪误差，建立x,y轴和旋转角三个运动方向的轨迹跟踪误差状态方程，康复步行训练机器人实际行走轨迹X(t)，医生指定训练轨迹Xd(t)，运动轨迹和运动速度跟踪误差e1(t)和e2(t)分别为e1(t)＝X(t)‑Xd(t)                                (2)其中X(t)＝[x(t) y(t) θ(t)]T表示x,y轴和旋转角方向的实际运动轨迹，Xd(t)＝[xd(t) yd(t) θd(t)]T表示x,y轴和旋转角方向的期望跟踪运动轨迹；e1(t)＝[e11(t) e12(t) e13(t)]T表示x,y轴和旋转角方向的轨迹跟踪误差，e2(t)＝[e21(t) e22(t) e23(t)]T表示x,y轴方向速度和旋转角速度的跟踪误差；步骤3)设置非线性控制器：其中K_d＝diag{K_d1,K_d2,K_d3}，K_p＝diag{K_p1,K_p2,K_p3}，表示B(θ)的伪逆矩阵，K_d,K_p表示控制器参数矩阵；将控制器公式(4)代入系统模型公式(1)，得由式(2)、(3)、(5)可得x,y轴和旋转角方向轨迹跟踪误差系统状态方程为步骤4)构建各轨迹跟踪误差系统渐近稳定的约束条件，获得非线性控制器参数矩阵的求解方法，使康复步行训练机器人暂态阶段和稳态阶段的实际运动轨迹控制在指定范围内；设置Lyapunov函数：其中沿轨迹跟踪误差系统(6)对式(8)求导，在如下约束条件成立时，有成立，即x,y轴和旋转角三个运动方向的轨迹跟踪误差系统渐近稳定，其中εi表示任意指定的小正数；步骤5)构建各轨迹跟踪误差系统渐近稳定的约束条件，获得非线性控制器参数矩阵的求解方法，使康复步行训练机器人暂态阶段和稳态阶段的实际运动轨迹约束在指定范围内；对式(10)两端从0到t积分，得进一步整理得Vi(t)‑Vi(0)≤‑εi(|e1i(t)|‑|e1i(0)|)                       (12)当如下条件成立时有下式成立由式(8)、(14)、(12)得于是有|e1i(t)|≤εi+|e1i(0)|                        (16)康复步行训练机器人的实际轨迹X(t)的安全运动范围为|x(t)‑xd(t)|≤ε1+|x(0)‑xd(0)|            (17)|y(t)‑yd(t)|≤ε2+|y(0)‑yd(0)|                   (18)|θ(t)‑θd(t)|≤ε2+|θ(0)‑θd(0)|                   (19)；基于MSP430系列单片机将输出PWM信号提供给电机驱动单元，使康复步行训练机器人的轨迹跟踪误差限制在指定范围内；以MSP430系列单片机为主控制器，主控制器的输入接电机测速模块、输出接电机驱动模块；电机驱动电路与直流电机相连；电源系统给各个电气设备供电；所述主控制器控制方法为：读取电机编码器的反馈信号与主控制器给定的控制命令信号X_d(t)和计算得出误差信号；根据误差信号，主控制器按照预定的控制算法计算出电机的控制量，送给电机驱动单元，电机转动带动轮子维持自身平衡及按指定方式运动；根据康复步行训练机器人对医生指定训练轨迹的运动速度和运动位置跟踪误差，设计非线性控制器，结合动力学模型，建立x,y轴和旋转角三个运动方向的轨迹跟踪误差状态方程；设计Lyapunov函数，构建各轨迹跟踪误差系统渐近稳定的约束条件，获得非线性控制器参数矩阵的求解方法，使康复步行训练机器人暂态阶段和稳态阶段的实际运动轨迹约束在指定范围内；基于MSP430系列单片机将输出PWM信号提供给电机驱动单元，使康复步行训练机器人在安全位置范围内跟踪医生指定的训练轨迹。