[发明专利]一种基于惯性模块的移动机器人力控算法

摘要

本发明提供了一种基于惯性模块的移动机器人力控算法，利用安装在机器人腰部的惯性模块获取机器人的姿态角；根据获得的姿态角判断机器人的速度，将速度给机器人，控制机器人运动。本发明使用成本低的弹簧加惯性装置模块实现对外力的大小和方向的判断，进而识别人的意图，实现外力控制；在惯性装置测量的机器人倾角变化到速度关系映射过程中，采用结合机器人历史及当前状态的判断方法，有效去除了弹簧的固有阻尼振荡；在惯性装置测量的机器人倾角变化到速度关系映射过程中，加入了运动学限制，将机器人的速度限制在一个范围内，一方面避免了运动中弹簧抖动的误判，另一方面避免刚度相对较小的弹簧，因形变较大而造成的速度过快。

主权项

1.一种基于惯性模块的移动机器人力控算法，利用安装在机器人腰部的惯性模块获取机器人的姿态角，通过函数映射关系将姿态角转变成与之对应的速度，其特征在于，具体包括：S1.机器人初始处于静止状态，当某一姿态角θ小于响应阈值θ₁时，与该姿态角对应的速度v＝0；当θ₁≤θ≤θ_max时，速度与姿态角成函数关系，即v＝f₁(θ)，并且v_max＝f₁(θ_max)，其中，f₁(θ)＝ke^b(θ+a)+c，式中的固定参数通过实验选取；当|θ|>|θ_max|时，机器人运动方向上速度不再变化；当与姿态角对应的速度不再变化时，姿态角接下来的变化只有两种可能，一种是失去外力，弹簧经历阻尼震荡后恢复静止状态，另一种则是对弹簧施加反相作用力：当|θ|>|θ₂|(θ₁·θ₂<0)时，θ₂为逆向响应阈值，v减小但不为0，目的是防止弹簧自由震荡引起的角度变化，并且满足公式v＝f₂(θ)，且f₂(θ₂)＝0，其中，f₂(θ)＝ke^b(θ+a)+c，式中的固定参数通过实验选取；S2.当|θ|≥|θ₂|，并且|θ|≤|θ_min|，v＝f₃(θ)，并且‑v_max＝f₃(θ_min)，其中f₃(θ)＝c′‑k′e^{b′(a′‑θ)}，式中的固定参数通过实验选取；当|θ|>|θ_min|时，机器人运动方向上速度不再变化；当与姿态角对应的速度不再变化时，姿态角接下来的变化只有两种可能，一种是失去外力，弹簧经历阻尼震荡后恢复静止状态，另一种则是对弹簧施加反相作用力：当θ<θ₁时，|v|增大但不为0，目的是防止弹簧自由震荡引起的角度变化，并且满足公式v＝f₄(θ)，且f₄(θ₁)＝0，其中，f₄(θ)＝c′‑k′e^{b′(a′‑θ)}，式中的固定参数通过实验选取。