こんにちは。koraです。
今回は 3次元線形倒立振子モードを使った歩行パターン生成について の続きで、歩行シミュレータを実装します。
歩行パターンの生成には、歩きながら実時間で計算する方法と、予め動作を計算しておく方法があります。ロボットが歩いている間も周囲の状況を認識しながら動作する場合や、人が操作する場合は前者が良さそうですが、HAC の競技は静止したボールを触るというルールなので後者でも問題なさそうです。できるだけシンプルな実装にしたいので、後者の方法で進めたいと思います。
シミュレータのアルゴリズム
アルゴリズムは、書籍ヒューマノイドロボット(第二版)の4.3.2節で解説されているものを参考にします。図に表すと次のようになります。
- 歩行周期、歩幅データ、初期重心位置、初期着地位置を設定する
- 線形倒立振子の方程式を積分して1歩行周期後の重心座標と重心速度を求める
- 次の目標着地点と、重心座標と重心速度の目標値を求める
- 次の修正した着地位置を求めて、実際の着地位置を更新する
- 2に戻る
歩行シミュレータの実装
早速、線形倒立振子のシミュレーションを実行する Python スクリプトを作成します。
3次元線形倒立振子のシミュレーションに ROS は必要ないのですが、結果の描写に RViz を使用するためです。
初期化処理
まず、以下が図中①の初期化処理です。
#!/usr/bin/env python # coding: utf-8 import math import numpy as np import rospy class SimpleSimulator(object): def __init__(self, step_time, ref_step_data, initial_centroid_position, initial_step_position): self.gravity = 9.8 # m/s2 self.delta_time = 0.01 # s self.centroid_height = 0.5 # m self.step_time = step_time self.ref_step_data = np.array(ref_step_data, dtype=np.float32) self.centroid_position = np.array(initial_centroid_position, dtype=np.float32) self.centroid_velocity = np.array([0, 0], dtype=np.float32) self.step_position = np.array(initial_step_position, dtype=np.float32) self.ref_step_position = np.array(initial_step_position, dtype=np.float32) self.step_count = 0 # ROS rospy.init_node('simple_simulator') self.vis_pub = rospy.Publisher('/centroid', Marker, queue_size=10) self.r = rospy.Rate(1.0 / self.delta_time)
- gravity は重力加速度です。
- delta_time は数値積分に用いる微小時間です。
- centroid_height は重心の高さです。シミュレーションの単純にするため高さ一定とします。
- step_time は片足の支持期間です
- ref_step_data は歩幅データです。1歩ごとにX方向の歩幅、Y方向の歩幅、向きを設定し、任意の歩数分設定します。
- centroid_position は現在の重心座標です。
- centroid_velocity は現在の重心速度です。初期状態では速度0とします。
- ref_step_position は現在の目標着地点です。
- step_position は現在の支持脚の実際の位置です。初期状態では ref_step_position と同じにします。
- step_count は歩数のカウンターです。初期状態は0歩目とします。
- rospy.init_node() で ROS ノードとして初期化しています。
- vis_pub は ROS パブリッシャーで、シミュレーション結果を /centroid というトピック名でパブリッシュします。
描写用の関数
シミュレーション結果を RViz に描写する関数を用意します。ここでは マーカーメッセージ を使用して球体を表示します。
# def publish_marker(self, idx, position, scale, color=(1.0, 0.0, 0.0, 1.0)): marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.ns = 'inverted_pendulum' marker.id = idx marker.action = Marker.ADD marker.pose.position.x = position[0] marker.pose.position.y = position[1] marker.pose.position.z = position[2] marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 1.0 marker.pose.orientation.w = 0.0 marker.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] marker.lifetime = rospy.Duration() marker.type = Marker.SPHERE self.vis_pub.publish(marker)
シミュレーション用の関数
以下がシミュレーション用の関数になります。
# def spin(self): for _ in range(len(self.ref_step_data) + 1): # 支持脚の描写 position = [self.step_position[0], self.step_position[1], 0.] self.publish_marker(idx=0, position=position, scale=(0.02,0.02,0.01)) # 線形倒立振子の方程式を積分して1歩分の重心運動を求める for i in range(int(self.step_time/self.delta_time)): centroid_acceleration = self.gravity / self.centroid_height * (self.centroid_position - self.step_position) self.centroid_velocity += centroid_acceleration * self.delta_time self.centroid_position += self.centroid_velocity * self.delta_time # 重心運動の描写 position = [self.centroid_position[0], self.centroid_position[1], self.centroid_height] self.publish_marker(idx=1, position=position, scale=(0.04,0.04,0.04)) self.r.sleep() # 1歩更新 self.step_count += 1 # 次の目標着地点を求める if self.step_count - 1 >= len(self.ref_step_data): current_ref_step_data = np.array([0, 0, 0], dtype=np.float32) else: current_ref_step_data = self.ref_step_data[self.step_count - 1] current_ref_step_angle = current_ref_step_data[2] current_ref_step_size = np.array([ current_ref_step_data[0] * np.cos(current_ref_step_angle) - current_ref_step_data[1] * np.sin(current_ref_step_angle), current_ref_step_data[0] * np.sin(current_ref_step_angle) + current_ref_step_data[1] * np.cos(current_ref_step_angle)]) self.ref_step_position = self.ref_step_position + current_ref_step_size # 次の1歩行周期後の重心座標と重心速度の目標値を求める if self.step_count >= len(self.ref_step_data): next_ref_step_data = np.array([0, 0, 0], dtype=np.float32) else: next_ref_step_data = self.ref_step_data[self.step_count] next_ref_step_angle = next_ref_step_data[2] next_ref_step_size = np.array([ next_ref_step_data[0] * np.cos(next_ref_step_angle) - next_ref_step_data[1] * np.sin(next_ref_step_angle), next_ref_step_data[0] * np.sin(next_ref_step_angle) + next_ref_step_data[1] * np.cos(next_ref_step_angle)]) Tc = math.sqrt(self.centroid_height / self.gravity) C = math.cosh(self.step_time / Tc) S = math.sinh(self.step_time / Tc) h_xy = next_ref_step_size / 2 h_v_xy = np.array(( (C + 1) / (Tc * S) * h_xy[0], (C - 1) / (Tc * S) * h_xy[1] ), dtype=np.float32) next_target_position = self.ref_step_position + h_xy next_target_velocity = h_v_xy # 次の修正した着地点を求める a = 10. b = 1. D = a * (C - 1)**2 + b * (S / Tc)**2 xyi = self.centroid_position vxyi = self.centroid_velocity xyd = next_target_position vxyd = next_target_velocity next_step_position = -a * (C - 1) / D * (xyd - C * xyi - Tc * S * vxyi) - b * S / (Tc * D) * (vxyd - S / Tc * xyi - C * vxyi) self.step_position = next_step_position
この関数は、次で示すように内部で「歩幅データの数 + 1歩」だけ処理を繰り返します。
def spin(self): for _ in range(len(self.ref_step_data) + 1): # 支持脚の描写 position = [self.step_position[0], self.step_position[1], 0.] self.publish_marker(idx=0, position=position, scale=(0.02,0.02,0.01))
以下が上図②の1歩分の重心運動を求める処理です。微小時間ずつ重心の状態を更新し、同時に重心の位置を描写します。
# 線形倒立振子の方程式を積分して1歩分の重心運動を求める for i in range(int(self.step_time/self.delta_time)): centroid_acceleration = self.gravity / self.centroid_height * (self.centroid_position - self.step_position) self.centroid_velocity += centroid_acceleration * self.delta_time self.centroid_position += self.centroid_velocity * self.delta_time # 重心運動の描写 position = [self.centroid_position[0], self.centroid_position[1], self.centroid_height] self.publish_marker(idx=1, position=position, scale=(0.04,0.04,0.04)) self.r.sleep()
以下が上図③の処理です。目標着地点 self.ref_step_position を更新します。これは単純に歩幅データから今度の歩幅 current_ref_step_size を求めて現在の目標着地点に足しているだけです。
# 1歩更新 self.step_count += 1 # 次の目標着地点を求める if self.step_count - 1 >= len(self.ref_step_data): current_ref_step_data = np.array([0, 0, 0], dtype=np.float32) else: current_ref_step_data = self.ref_step_data[self.step_count - 1] current_ref_step_angle = current_ref_step_data[2] current_ref_step_size = np.array([ current_ref_step_data[0] * np.cos(current_ref_step_angle) - math.pow(-1, self.step_count) * current_ref_step_data[1] * np.sin(current_ref_step_angle), current_ref_step_data[0] * np.sin(current_ref_step_angle) + math.pow(-1, self.step_count) * current_ref_step_data[1] * np.cos(current_ref_step_angle)]) self.ref_step_position = self.ref_step_position + current_ref_step_size
また、1歩後の重心座標 next_target_position と重心速度の目標値 next_target_velocity を求めます。これは歩幅データから1歩後の歩幅 next_ref_step_size を求め、さらに歩行素片を用いて目標とする重心の状態を計算しています。
# 次の1歩後の重心座標と重心速度の目標値を求める if self.step_count >= len(self.ref_step_data): next_ref_step_data = np.array([0, 0, 0], dtype=np.float32) else: next_ref_step_data = self.ref_step_data[self.step_count] next_ref_step_angle = next_ref_step_data[2] next_ref_step_size = np.array([ next_ref_step_data[0] * np.cos(next_ref_step_angle) - (-1) * math.pow(-1, self.step_count) * next_ref_step_data[1] * np.sin(next_ref_step_angle), next_ref_step_data[0] * np.sin(next_ref_step_angle) + (-1) * math.pow(-1, self.step_count) * next_ref_step_data[1] * np.cos(next_ref_step_angle)]) Tc = math.sqrt(self.centroid_height / self.gravity) C = math.cosh(self.step_time / Tc) S = math.sinh(self.step_time / Tc) h_xy = next_ref_step_size / 2 h_v_xy = np.array(( (C + 1) / (Tc * S) * h_xy[0], (C - 1) / (Tc * S) * h_xy[1] ), dtype=np.float32) next_target_position = self.ref_step_position + h_xy next_target_velocity = h_v_xy
以下が上図④の修正した着地位置を求める処理です。積分で求めた現在の重心座標 centroid_position と重心速度 centroid_velocity 、および歩行素片から求めた1歩後の重心座標 next_target_position と重心速度 next_target_velocity の目標値から、修正した着地位置を計算して実際の着地点 self.step_position を更新します。
# 次の修正した着地点を求める a = 10. b = 1. D = a * (C - 1)**2 + b * (S / Tc)**2 xyi = self.centroid_position vxyi = self.centroid_velocity xyd = next_target_position vxyd = next_target_velocity next_step_position = -a * (C - 1) / D * (xyd - C * xyi - Tc * S * vxyi) - b * S / (Tc * D) * (vxyd - S / Tc * xyi - C * vxyi) self.step_position = next_step_position
以上の処理を繰り返して、歩行パターンを作成します。
シミュレーションを実行
片足の支持期間、歩幅データ、初期状態を指定します。0.5 秒で脚を切り替え、歩幅は X 方向に 0.1 m、Y 方向に 0.1 m、向きは常に 0 rad 方向として 8 歩だけ歩かせる場合、次のようになります。
if __name__ == '__main__': simulator = SimpleSimulator( step_time = 0.5, ref_step_data = [ [0.1, 0.1, 0.0], [0.1, 0.1, 0.0], [0.1, 0.1, 0.0], [0.1, 0.1, 0.0], [0.1, 0.1, 0.0], [0.1, 0.1, 0.0], [0.1, 0.1, 0.0], [0.1, 0.1, 0.0], [0.1, 0.1, 0.0]], initial_centroid_position = [0, 0.04], initial_step_position = [0, 0.05] ) simulator.spin()
実際にシミュレーションを実行した結果、以下のような歩行パターンが得られました。左右に揺れながら進んでいる赤点が重心で、地面に現れたり消えたりしている赤点が着地位置です。
おわりに
目標とする歩幅を入力することで、ロボットの重心の運動と着地点の位置を求めることができるようになりました。実際にこれをロボットで再現させるには、ロボット重心の座標と足の座標から関節角度を逆算してサーボモーターに指示する必要があります。この処理については次回以降取り扱いたいと思います。