system: [SYSTEM]

user: 5. 仿生粘附式尺蠖机器人步态规划 5.1. 机器人姿态和轨迹规划 用几何约束法进行步态规划，首先要规划好尺蠖机器人的行走姿态和轨迹。在本论文中，研究尺蠖机器人的“一字”步态：机器人两脚在目标平面的落脚痕迹在一条直线上。在“一字”步态下机器人的关节J1和J6不动，对应的关节转角 θ 1 = θ 6 = 90 ∘ 且始终不变。因此，机器人的自由度由6个变为4个。整个步态过程如图11所示。 一个尺蠖步态周期的运动过程为： 图11. 尺蠖步态示意图 1) 初始步态，尺蠖机器人两吸盘贴于目标平面。 2) 足S1保持吸附状态，足S2的吸盘脱附，关节J2、J3、J4和J5配合运动使足S2向上向前运动。 3) 关节J2、J3、J4和J5配合运动使足S2向下向前运动，等足S2运动至合适位置时，吸盘重新黏附到目标平面。 4) 足S2保持吸附状态，足S1的吸盘脱附，关节J2、J3、J4和J5配合运动使足S1向上向前运动。 5) 关节J2、J3、J4和J5配合运动使足S1向下向前运动，等足S1运动至合适位置时，吸盘重新黏附到目标平面。 考虑到黏附吸盘的尺寸较大，为了避免干涉和方便吸盘进行黏附控制，规划在行走过程中吸盘平面始终与目标平面保持平行，此时有： θ 2 + θ 3 + θ 4 + θ 5 = − 180 ∘ (5-1) 本论文中的尺蠖机器人存在冗余的自由度，为了使步态规划中运动学方程解的唯一性，需要引入额外约束降低尺蠖机器人的自由度。限定尺蠖机器人在运动过程中关节J3和J4始终处于对称状态，即有： θ 3 = θ 4 (5-2) 5.2. 关节运动方程求解 5.2.1. 运动学方程的建立 在尺蠖机器人原有运动学模型的基础上，引入约束，可以建立约束条件下的运动学方程。 尺蠖机器人右脚相对左脚的坐标变化矩阵为： A 0 6 = A 0 1 A 1 2 A 2 3 A 3 4 A 4 5 A 5 6 = [ x ^ 1 y ^ 1 z ^ 1 p 1 x ^ 2 y ^ 2 z ^ 2 p 1 x ^ 3 y ^ 3 z ^ 3 p 3 0 0 0 1 ] (5-3) 其中， [ x ^ 1 y ^ 1 z ^ 1 x ^ 2 y ^ 2 z ^ 2 x ^ 3 y ^ 3 z ^ 3 ] 为坐标系 O 6 X 6 Y 6 Z 6 相对于坐标系 O 0 X 0 Y 0 Z 0 的其次坐标变化矩阵， [ p 1   p 2   p 3 ] Τ 为 坐标原点 O 6 在坐标系 O 0 X 0 Y 0 Z 0 中的表达式。 根据步态规划，机器人在行走过程中两个吸盘平面保持平行，且与目标平面平行，可知： A 0 6 = A 0 1 A 1 2 A 2 3 A 3 4 A 4 5 A 5 6 = [ x ^ 1 y ^ 1 z ^ 1 p 1 x ^ 2 y ^ 2 z ^ 2 p 1 x ^ 3 y ^ 3 z ^ 3 p 3 0 0 0 1 ] = [ 1 0 0 p 1 0 1 0 p 1 0 0 1 p 3 0 0 0 1 ] (5-4) 且有： θ 2 + θ 3 + θ 4 + θ 5 = − 180 ∘ 将式有关表达式代入 A 0 6 = A 0 1 A 1 2 A 2 3 A 3 4 A 4 5 A 5 6 中，并限定 θ 3 = θ 4 ，经过化简可以得到： { p 1 = 0 p 2 = − a 1 − a 2 cos θ 2 − a 3 cos ( θ 2 + θ 3 ) − a 4 cos ( θ 2 + θ 3 + θ 4 ) + a 5 p 3 = − a 2 sin θ 2 − a 3 sin ( θ 2 + θ 3 ) − a 4 sin ( θ 2 + 2 θ 3 ) (5-5) 仿生粘附式尺蠖机器人一个完整的步态主要包括右脚迈步和左脚迈步两个过程。设尺蠖机器人初始状态下各关节的转角为 θ 2 = − 30 ∘ 、 θ 3 = − 60 ∘ 、 θ 4 = − 60 ∘ 、 θ 5 = − 30 ∘ ，此时可以求得左右脚在水平方向的距离为 a 2 + a 3 。 当右脚往前迈时 θ 2 ， θ 3 ， θ 4 ， θ 5 关节的运动轨迹可用下列方程组描述： { p 1 = 0 p 2 = − 7 t p 3 = a 2 + a 3 + 7 t       t ∈ ( 0 ， 3   s ) (5-6) { p 1 = 0 p 2 = − 21 + 7 ( t − 3 ) p 3 = a 2 + a 3 + 21 + 7 ( t − 3 )     t ∈ ( 3   s ， 6   s ) (5-7) 当左脚往前迈时 θ 2 ， θ 3 ， θ 4 ， θ 5 关节的运动轨迹可用下列方程组描述： { p 1 = 0 p 2 = 7 ( t − 6 ) p 3 = a 2 + a 3 + 42 − 7 ( t − 6 )       t ∈ ( 6   s ， 9   s ) (5-8) { p 1 = 0 p 2 = 21 − 7 ( t − 9 ) p 3 = a 2 + a 3 + 21 − 7 ( t − 9 )           t ∈ ( 9   s ， 12   s ) (5-9) 5.2.2. 运动学方程的求解 当 t ∈ ( 0 ， 3   s ) 时，由式(5-5)和(5-6)有： − 7 t = − a 1 − a 2 cos θ 2 − a 3 cos ( θ 2 + θ 3 ) − a 4 cos ( θ 2 + 2 θ 3 ) + a 5 (5-10) a 2 + a 3 + 7 t = − a 2 sin θ 2 − a 3 sin ( θ 2 + θ 3 ) − a 4 sin ( θ 2 + 2 θ 3 ) (5-11) 由于 a 1 = a 2 = a 4 = a 5 ，化简可得： 7 t = a 2 cos θ 2 + a 3 cos ( θ 2 + θ 3 ) + a 2 cos ( θ 2 + 2 θ 3 ) (5-12) a 2 + a 3 + 7 t = − a 2 sin θ 2 − a 3 sin ( θ 2 + θ 3 ) − a 2 sin ( θ 2 + 2 θ 3 ) (5-13) 式(5-12)和(5-13)两边平方后相加，并利用 cos 2 θ 3 = 2 cos 2 θ 3 − 1 ，化简可得： 4 a 2 2 cos 2 θ 3 + 4 a 2 a 3 cos θ 3 + a 3 2 − 49 t 2 − ( a 2 + a 3 + 7 t ) 2 = 0 (5-14) 解得： cos θ 3 = − 4 a 2 a 3 ± 16 a 2 2 a 3 2 − 16 a 2 2 [ a 3 2 − 49 t 2 − ( a 2 + a 3 + 7 t ) 2 ] 8 a 2 2 = − a 3 2 a 2 ± 49 t 2 + ( a 2 + a 3 + 7 t ) 2 2 a 2 (5-15) 因为 − a 3 2 a 2 = − 155.42 2 × 70.61 ≈ − 1.1 ，故负号舍去。 若 θ 3 有解，则需满足 − 1 ≤ cos θ 3 ≤ 1 ，即： − 1 ≤ − a 3 2 a 2 + 49 t 2 + ( a 2 + a 3 + 7 t ) 2 2 a 2 ≤ 1 ， 解得 0 ≤ t ≤ 17.0 ，符合实际情况，故可得： θ 3 = arccos ( − a 3 + 49 t 2 + ( a 2 + a 3 + 7 t ) 2 2 a 2 ) = θ 4 (5-16) 由 ( 5 - 12 ) × cos θ 2 和 ( 5 -13 ) × sin θ 2 可得： 7 t cos θ 2 = a 2 cos 2 θ 2 + a 3 cos θ 2 cos ( θ 2 + θ 3 ) + a 4 cos θ 2 cos ( θ 2 + 2 θ 3 ) (5-17) ( a 2 + a 3 + 7 t ) s i n θ 2 = − a 2 sin 2 θ 2 − a 3 s i n θ 2 sin ( θ 2 + θ 3 ) − a 2 s i n θ 2 sin ( θ 2 + 2 θ 3 ) (5-18) 两式相减，可得： 7 t cos θ 2 − ( a 2 + a 3 + 7 t ) s i n θ 2 = a 2 + a 3 cos θ 3 + a 2 cos 2 θ 3 (5-19) 即： − ( a 2 + a 3 + 7 t ) s i n θ 2 + 7 t cos θ 2 = a 2 + a 3 ( − a 3 + 49 t 2 + ( a 2 + a 3 + 7 t ) 2 2 a 2 )     + a 2 [ 2 ( − a 3 + 49 t 2 + ( a 2 + a 3 + 7 t ) 2 2 a 2 ) 2 − 1 ] = 49 t 2 + ( a 2 + a 3 + 7 t ) 2 − a 3 49 t 2 + ( a 2 + a 3 + 7 t ) 2 2 a 2 (5-20) 令： k 1 = − ( a 2 + a 3 + 7 t ) k 2 = 10 t k 3 = 49 t 2 + ( a 2 + a 3 + 7 t ) 2 − a 3 49 t 2 + ( a 2 + a 3 + 7 t ) 2 2 a 2 则有： θ 2 = arctan 2 ( k 1 k 2 ) ± arctan 2 ( k 1 2 + k 2 2 − k 3 2 k 3 ) (5-21) 为了使 θ 2 为负号且符合要求，取 θ 2 = arctan 2 ( k 1 k 2 ) + arctan 2 ( k 1 2 + k 2 2 − k 3 2 k 3 ) (5-22) 此外，可以求得关节J5的转角为： θ 5 = π − θ 2 − θ 3 − θ 4   = π − [ arctan 2 ( k 1 k 2 ) + arctan 2 ( k 1 2 + k 2 2 − k 3 2 k 3 ) ]     − 2 arccos ( − a 3 + 49 t 2 + ( a 2 + a 3 + 7 t ) 2 2 a 2 ) (5-23) 同理可以求得 t ∈ ( 3   s ， 6   s ) ， t ∈ ( 6   s ， 9   s ) ，和 t ∈ ( 9   s ， 12   s ) 时的运动学方程。 5.3. 虚拟样机建模及仿真 Adsms软件是一款常用的虚拟样机技术软件，可以用于运动学和动力学等的仿真。运用ADAMS软件对尺蠖机器人进行运动学仿真，可以验证运动学方程和步态规划结果的正确性。在软件中建立的模型如图12所示。 设定尺蠖机器人的初始位置为 θ 2 = − 30 ∘ 、 θ 3 = − 60 ∘ 、 θ 4 = − 60 ∘ 、 θ 5 = − 30 ∘ ，采用步态规划中的“一字”步态进行仿真。在4个关节上设置4个运动副，并设关节的转角为运动变量，将5.2节计算得到的关节转角随时间变化的方程作为机器人的运动函数。设定仿真时间为13 s、步长为0.1后开始仿真，在Adams中自动生成尺蠖机器人行走的动画，如图13所示。从图中可以看出，尺蠖机器人按照规划的步态能够实现在平面上稳定的行走。仿真结果证明了运动学建模和步态规划结果的准确性。 利用Adams的角度测算功能，可以得到仿真过程中关节J2、J3、J4、J5的转角(相对值)随时间的变化曲线如图14。 4个关节转角曲线图是相对于尺蠖机器人的初始位置的关节转角。从曲线图中可以看出，在运动过程中可以看出，四个关节的转角之和始终为0，考虑到初始位置有 θ 2 + θ 3 + θ 4 + θ 5 = − 180 ∘ ，故运动过程中时刻满足 θ 2 + θ 3 + θ 4 + θ 5 = − 180 ∘ ，且由动画可以看出尺蠖机器人的两个吸盘底部平面始终平行，与步态规划结果一致。 图12. Adams软件中的尺蠖机器人模型 图13. Adams中尺蠖机器人尺蠖步态动画 图14. 关节2、3、4、5转角曲线图