单腿跳跃机器人轨迹规划(2)

　　·
　　ｆｌ
　　ｆ０ ＝ ［０ ０］Ｔ，Ｐ¨ｆｌ
　　ｆ０ ＝［０ ０］Ｔ。
　　腾空段踝关节不提供力矩，是一个欠驱动过
　　程，三个关节角不是相互独立的。运用参数化方法假定关节角１和关节角２按如下规律进行变化：
　　θｊ（ｔ）＝ａｊ１＋ａｊ２ｔ＋ａｊ３ｔ２＋ａｊ４ｔ３＋ａｊ５ｔ４＋ａｊ６ｔ５＋ａｊ７ｔ３（ｔ－ｔｆｌ）３ （１０）ｊ＝１，２
　　注意到，最后一个系数的改变不影响角度、角
　　速度、角加速度的边界值，它的存在用于改变曲线的形状。上述２个关节角共１４个系数，加上关节角３初始时刻角速度θ
　　· ｆｌ
　　３，０
　　共１５个未知参数。
　　腾空段外力只有重力，所以其关于质心的角
　　动量守恒。由此可得关系式
　　ｄ　Ｈ
　　ｄｔ ＝ｇ（Θｆｌ，Θ
　　· ｆｌ，Θ¨ｆｌ）＝０ （１１）
　　将整个腾空时间离散化，假设分为Ｎ 个时间
　　间隔Δｔ，将第ｋ个时间间隔内的Θｆｌ
　　ｋ、Θ
　　· ｆｌ
　　ｋ
　　和θ ¨
　　１，ｋ、θ ¨
　　２，ｋ
　　代入式（１１）中可以得到θ ¨
　　３，ｋ，运用欧拉公式进行
　　数值积分得到
　　θ ·
　　３，ｋ＋１ ＝θ
　　·
　　３，ｋ ＋θ
　　¨
　　３，ｋΔｔ （１２）
　　θ３，ｋ＋１ ＝θ３，ｋ ＋θ
　　·
　　３，ｋΔｔ＋１２ θ
　　¨
　　３，ｋΔｔ２ （１３）
　　由式（１０）～ 式（１３）计算出所有关节角的角度、角速度和角加速度后，再由式（２）可以计算出每个采样时间内的膝关节和髋关节力矩τ２、τ３，踝关节力矩τ１ ＝０。
　　机器人在腾空段的优化模型如下：
　　目标函数：ｃ＝ １２
　　ＵＴＵΔｔ
　　Ｕ ＝
　　［τ１（１）τ２（１）τ３（１） … τ１（Ｎ）τ２（Ｎ）τ３（Ｎ）］Ｔ等式约束：
　　［θ１（ｔ＝０）θ２（ｔ＝０）］Ｔ ＝ θｆｌ［１０ θｆ２ｌ０］
　　Ｔ
　　［θ１（ｔ＝ｔｆｌ）θ２（ｔ＝ｔｆｌ）］Ｔ ＝ θｆｌ［１ｆ θｆ２ｌｆ］
　　Ｔ
　　Ｐ ·
　　ｆｌ
　　ｃ０ ＝ ［ｖｃｘ ｖｃｙ０］
　　Ｔ
　　Ｐ¨ｆｌ
　　ｃ０ ＝ ［０ －ｇ　］Ｔ　　θ３，Ｎ＋１ ＝θｆｌ３ｆ
　　不等式约束：
　　τ２ｌ≤τ２ ≤τ２ｕ　　τ３ｌ≤τ３ ≤τ３ｕθｉｌ≤θｉ ≤θｉｕ　　ｉ＝１，２，３
　　θ２３ｌ≤θ２３ ≤θ２３ｕ
　　ｙｆｌ
　　ｆ（ｋ）＞０　　ｋ＝１，２，…，Ｎ －１
　　其中，ｙｆｌ
　　ｆ
　　表示腾空段时脚在惯性系中的竖直方向
　　位移。腾空段过程是运用ＭＡＴＬＡＢ中的ｆｍｉｎｃｏｎ函数寻优，以下各段均与此相同。
　　站立段的优化模型与此类似，考虑到跳跃过
　　程中的碰撞现象和周期性特点，站立段开始时刻状态就是机器人脚与地面发生碰撞之后的状态，站立段终了时刻状态就是机器人腾空前一时刻的状态，所以当腾空段的状态确定以后，这两个时刻的状态也就被确定了，即已知Θｓｔ
　　０、Θ
　　· ｓｔ
　　０、Θｓｔ
　　ｆ、Θ
　　· ｓｔ
　　ｆ（其
　　中上标ｓｔ表示站立段）。假设站立段质心前向平均速度与腾空段质心前向速度相同，由式（１）可得站立段质心开始时刻和终了时刻的位置Ｐｓｔｃ０、
　　Ｐｓｔ
　　ｃｆ，那么站立段时间为
　　ｔｓｔ＝
　　ｘｓｔ
　　ｃｆ－ｘｓｔ
　　ｃ０
　　ｖｃｘ
　　（１４）
　　站立段是全驱动状态，假设三个关节角都按
　　式（１０）的规律变化，目标函数与腾空段相同，优化模型的约束如下：