UMEHOSHI ITA TOP PAGE

[Raspberry Pi 3 Model A+]と[UMEHOSHI ITA]を乗せたモータ付き台車を使った実験

このページで示した[Raspberry Pi]のサービスのstart-app-select.pyから, DIPスイッチ0b10??の状態で呼び出されるraspiAPumeTest.pyのコードです。
また、これで使うハードとその動作確認コードは、このリンク先でで紹介しています。

ロボットの倒立振子の検討の記録

ここで使用するロボットは二輪車と、別途の一つの支柱が接地する構造で、電源を切っても倒れない構造になっている。
この状態から、支柱浮かせて二輪だけで動かす目標の検討です。

実験は、以前に作成した「TCPサーバー」のロボットに、 実験用のPythonファイルをロボットに送り込んで実行する繰り返しで行う。
送信したファイルは、「/usr/local/apps/」に送られる。
実験は、このフォルダに後述の「raspiAPumeTest.py」ファイルを送ることで行いました。
倒立振子を行う場合に必要な情報は、モータの前進や後進の方向に回転するPitch情報です。
現状で支柱が接地している場合はPitchが0.6です。そいて、支柱の接地が浮いた時はPitchが45まで回転できます。
モータを前進させ、その後でモータ逆回転させるとつんのめって支柱の接地が浮いてPitchが大きくなります。
この状態を保持させる制御を行います。
接地安定状態目標となる二輪車だけの倒立制御状態

最初の目標として、支柱の接地が浮いた時の目標角度の設定を「target_angle=23.5」として実験しました。
この角度は、上記の右写真の「二輪車だけの倒立制御状態」の角度で、前後で均衡がとりやすいと判断した時のPitch測定値です。
(Pitchの測定値は、ここで紹介した9軸センサーBNO055から得られる値で、キャリブレーションや オフセット調整済みの値ですが、
　　再びキャリブレーションを行うと変化してしまう可能性がある値です)
そして、単純に「Pitch < target_angle」であれば逆転させて、
「Pitch > target_angle」であれば正転させる制御をしてみました。

この制御に移行するため、最初にモータを前進させて助走した後、モータ逆回転で支柱浮かせています。
この判定も「Pitch > target_angle」で、最初はそうなるまで逆転させます。
最初の作った時の動作イメージを以下に示します。（下記クリックでXのムービに移動します。）

以下にこのコードを示します。
上記の動作は、以下のコードのmeasure_loop()関数において、 deadband = 1.0 # デッドバンドの設定を0にした挙動です。 
#!/usr/bin/python3
# -*- coding: utf-8 -*-
#   TCPサーバープログラム（/usr/local/apps/raspiAPumeTest.py）
#  [Raspberry Pi 3 Model A+]と[UMEHOSHI ITA]を乗せたモータ付き台車のサービスから呼び出される
# 倒立振子を試すコード

# ---- LED用の出力とタクトスイッチ用入力 のためのGPIO初期化-----
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(21, GPIO.OUT)  # GPIO21を出力に設定
GPIO.output(21, GPIO.HIGH)  # ON（3.3V）
for no in [6, 16, 17]:
    GPIO.setup(no, GPIO.IN, pull_up_down=GPIO.PUD_UP)  # プルアップ付き入力

#------ GPIO初期化 終了 -------------------------------------

import signal # システムが閉じることを判断するため追加 ★
import board
import busio
from adafruit_ssd1306 import SSD1306_I2C # SSD1306ディスプレイ用
import adafruit_vl53l1x # VL53L1X使用 レーザー測距センサーモジュール用
# オープンソースハードウェアの設計・製造・販売を行うアメリカの企業のAdafruit（エイダフルート）モジュール利用
from PIL import Image, ImageDraw, ImageFont
import time

i2c = busio.I2C(board.SCL, board.SDA)# --- I2C初期化 ---

# --- SSD1306ディスプレイ初期化 (128x64の場合) -----------
oled = SSD1306_I2C(128, 64, i2c)
oled.contrast(128) # 0?255

oled.fill(0)   # --- クリア
oled.show() # ---表示

# --- Pillowで描画領域を作成 ---
image = Image.new("1", (oled.width, oled.height))
draw = ImageDraw.Draw(image)
font = ImageFont.load_default()# --- フォント設定 ---

def draw_text(txt: str, row=0, showFlag = True, newImageFlag = False, font=font, fill=255):
   global image,draw
   if newImageFlag: 
      image = Image.new("1", (oled.width, oled.height)) # イメージ作り直し（全体クリア）
      draw = ImageDraw.Draw(image)
   # --- テキスト描画 (0＝黒、255＝白)上記設定で、横21文字---
   draw.text((0, row*15), txt , font=font, fill=255)
   # --- 画面に表示 ---
   oled.image(image)
   if showFlag: oled.show()

draw_text(f"UMEHOSHI ITA",0)

# 9軸センサー BNO055 制御 ---------------------------------------------------
import smbus # I2C通信をPythonから簡単に扱うためのモジュール
import os

# BNO055 の初期化
BNO055_ADDRESS = 0x28  # BNO055のI2Cアドレス(ADRピンがGNDなら0x28、VDDなら0x29になります)
BNO055_OPR_MODE = 0x3D # 動作モードを設定するためのレジスタ
BNO055_EULER_H_LSB = 0x1A # オイラー角（方位・ロール・ピッチ）のデータが始まるアドレス
bus = smbus.SMBus(1)# 引数の1でRaspberry PiのボードGPIO2: SDA、GPIO3: SCLを指定

bno055_calib_bin_path="/usr/local/apps/bno055_calib.bin" # キャリブレーションデータファイルパス
# このキャリブレーションデータファイルが存在しなければ、作成する。
try:
   os.stat(bno055_calib_bin_path) # ファイルが存在しない場合は、エラー
except OSError:
   bus.write_byte_data(BNO055_ADDRESS, BNO055_OPR_MODE, 0x00) # 設定変更(OPR_MODE)でCONFIGモードに切り替える
   time.sleep(0.05)
   # センサーをリセット（0x3FのSYS_TRIGGERレジスタのビット7をセット）
   bus.write_byte_data(BNO055_ADDRESS,0x3F, 0x20)
   time.sleep(0.7)  # リセット後は再起動まで時間がかかる
   # 出力単位(UNIT_SEL)を設定（0x00で「角度＝度（°）」単位）
   bus.write_byte_data(BNO055_ADDRESS,0x3B, 0x00)
   # センサーフュージョンを有効にするNDOFモードに変更
   bus.write_byte_data(BNO055_ADDRESS,BNO055_OPR_MODE, 0x0C) # NDOFモードへ
   time.sleep(0.05)
   while True: # NDOFモードで全てのセンサー（SYS, GYR, ACC, MAG）が 3 になるまで動かす。
      cal = bus.read_byte_data(BNO055_ADDRESS, 0x35)
      sys = (cal >> 6) & 0x03
      gyr = (cal >> 4) & 0x03
      acc = (cal >> 2) & 0x03
      mag = (cal >> 0) & 0x03
      draw_text(f"bno055 NDOF Calibrating...",0, showFlag = False,newImageFlag = True) # キャリブレーションの開始
      draw_text(f"SYS:{sys}, GYR:{gyr}, ACC:{acc}, MAG:{mag}", 1)
      time.sleep(0.5)
      if sys == 3 and gyr == 3 and acc == 3 and mag == 3 : break # キャリブレーション完了？
      # 各値が 3 になれば完全キャリブレーション完了です
   #
   calib_data = bus.read_i2c_block_data(BNO055_ADDRESS, 0x55, 22)
   with open(bno055_calib_bin_path, "wb") as f:
      f.write(bytearray(calib_data)) # オフセット値を読み出して保存
   #
   draw_text(f"End Calibration",2) # キャリブレーションデータファイル作成終了

# オフセット調整（センサーの取り付け位置や方向を自動補正）
bno055_offset_path="/usr/local/apps/bno055_offset.txt"
# 上記ファイル内に X軸が北を向く時にHeading、水平に置いた時にRollと Pitchが記憶される
make_offset_mode=False # "bno055_offset.txt"オフセット調整ファイル作成モード
try:
   with open(bno055_offset_path, "r") as fr:
      s = fr.readline() # 一行読み取り(改行を含めて)
   draw_text(f"bno055 offset setting",0,newImageFlag = True)
   a = s.split(",")
   heading_offset = float(a[0]) # オフセット調整値取得
   roll_offset = float(a[1])
   pitch_offset = float(a[2])
   print(f"offset value heading:{heading_offset}, roll:{roll_offset}, pitch:{pitch_offset}")
except:
   make_offset_mode = True # bno055_offset.txtのオフセット調整作成モード

bus.write_byte_data(BNO055_ADDRESS, BNO055_OPR_MODE, 0x00) # 設定変更(OPR_MODE)でCONFIGモードに切り替える
time.sleep(0.05)

with open(bno055_calib_bin_path, "rb") as f:
   calib_data = list(f.read(22)) # 別途bno055_calib_write.pyで行ったキャリブレーションの記憶ファイルを読む

# 設定モードへ
bus.write_byte_data(BNO055_ADDRESS, 0x3D, 0x00)
time.sleep(0.025)

# キャリブレーションデータを書き込んで、調整情報を復元
bus.write_i2c_block_data(BNO055_ADDRESS, 0x55, calib_data)

# NDOFモードに戻す
bus.write_byte_data(BNO055_ADDRESS, 0x3D, 0x0C)
time.sleep(0.05)

# 出力単位(UNIT_SEL)を設定（0x00で「角度＝度（°）」単位）
bus.write_byte_data(BNO055_ADDRESS,0x3B, 0x00)

def to_signed(val):
   """16ビット値を符号付き整数に変換"""
   if val >= 0x8000:
      val -= 0x10000
   return val


prev1_Pitch = 0 # 前のPitchの測定値
prev2_Pitch = 0 # 前のPitchの測定値
prev3_Pitch = 0 # 前のPitchの測定値
def read_euler():
   ''' make_offset_modeがTrueの場合は、現在の
   heading：左右への向き, roll：左右の傾き, pitch：上下の傾きを返す。
   make_offset_modeがFalseの場合は、調整過程のheading, roll, pitchを返す'''
   #
   global prev1_Pitch,prev2_Pitch,prev3_Pitch # 前のPitchの測定値
   data = bus.read_i2c_block_data(BNO055_ADDRESS, BNO055_EULER_H_LSB, 6)
   # 各要素が 1 バイト（0〜255）の整数を6個のリストで得られる。（1 LSB = 1/16 度）

   # データはリトルエンディアン形式（下位→上位の順）
   heading = (data[1] << 8) | data[0]  # 方位角（北基準のYAW）
   roll    = (data[3] << 8) | data[2]  # ロール角（左右の傾き）
   pitch   = (data[5] << 8) | data[4]  # ピッチ角（前後の傾き）

   # ロールとピッチは符号付き
   roll = to_signed(roll)
   pitch = to_signed(pitch)

   # スケーリング（1 LSB = 1/16 度）
   heading = heading / 16.0 # 
   roll    = roll / 16.0
   pitch   = pitch / 16.0

   if make_offset_mode == False: #オフセット調整処理
      # X軸が北を向く時にHeadingが0、水平に置いた時にRollと Pitchが0になるオフセット調整
      heading = heading - heading_offset # headingズレ補正
      heading = heading if heading >= 0 else heading + 360
      roll = roll - roll_offset # rollズレ補正
      if roll > 90:
         roll = -(90 - roll)
      elif roll < -90:
         roll = -(-90 - roll)
      pitch = pitch - pitch_offset # pitchズレ補正
      if pitch > 180:
         pitch = -(360 - pitch)
      elif pitch < -180:
         pitch = -(-360 - pitch)
   #
   prev3_Pitch = prev2_Pitch # 前の測定値記憶
   prev2_Pitch = prev1_Pitch # 前の測定値記憶
   prev1_Pitch = pitch       # 前の測定値記憶
   return heading, roll, pitch # 左右への向き, 左右の傾き, 上下の傾きを返す


if make_offset_mode: #オフセット調整処理
      count = 100 # ロボットを水平にしてはX軸が北を向いてから安定に必要な予想回数
      while True:
         h, r, p = read_euler()
         print(f"Heading: {h:7.2f}, Roll: {r:7.2f}, Pitch: {p:7.2f}")
         count -= 1
         print(f"  {count}が0になるまでに、ロボットを水平にしてはX軸が北を向くように置いてください。")
         draw_text(f"Before CountReaches 0",0,showFlag = False, newImageFlag = True)
         draw_text(f"Place it horizontally",1,showFlag = False)
         draw_text(f"   and point it north",2,showFlag = False)
         draw_text(f" count:{count}",3)
         time.sleep(0.2)
         if count <= 0: 
            with open(bno055_offset_path, "w") as fw:
               fw.write(f"{h},{r},{p}\n") # オフセット調整データ書き込み
            heading_offset, roll_offset,pitch_offset=h,r,p
            break # 上記でX軸が北を向く時にHeading、水平に置いた時にRollと Pitchを記憶

# --------------------------------------------------------------------------------
vl53 = adafruit_vl53l1x.VL53L1X(i2c)# VL53L1X使用 レーザー測距センサーモジュール初期化
print("VL53L1X Start measuring...")
vl53.start_ranging()

distance = vl53.distance
time.sleep(0.5)
print(f"Distance: {distance} mm")

# umetcp umeusb 通信関連----------------------------------------------------
import os
import umetcp
from umetcp import send_message, receiveData
import socket
import umeusb
import traceback

sock = None # クライアントと通信するソケット

def my_tcp_receive_file_func(filepath):
   ''' 受信したumehoshiアプリ用「.umh」のファイルより、
   「UME専用Hexコマンド」の文字列をusbへ出力する'''
   name,ext = os.path.splitext(filepath)
   if not ext == ".umh" : return
   with open( filepath ) as f: ss=f.readlines()
   ss="".join(ss[1:])
   print(ss)
   umeusb.send_cmd(ss, quantity=0) # TCPで受信した 「.umh」データを[UMEHOSHI ITA]へ送る

umetcp.tcp_receive_file_func = my_tcp_receive_file_func # tcpファイル受信データの処理を置き換え

def my_tcp_recieve_message(msg):
   umeusb.send_cmd(msg)

umetcp.tcp_receive_message_func=my_tcp_recieve_message  # TCO受信文字列（UME専用Hexコマンド）処理を置き換え

def my_usb_receive_func(bin):
   'usb受信のイベントで実行するデフォルト処理'
   global sock
   #print("-----------",bin)
   ss = bin.decode('utf-8')
   a=ss.split('\n')
   for s in a:
      s = s.strip()
      if s == "": continue 
      if sock != None:
         print(f"USB receive:{s}")
         send_message(sock, s) # [UMEHOSHI ITA]からの応答メッセージをTCPで返す
      else: print( f"USB receive:{s}" )

umeusb.usb_receive_func = my_usb_receive_func # USB受信データの処理を置き換える。

umeusb.init_sub()
import threading
t_id = threading.Thread(target=umeusb.read_loop)
t_id.start()

#ip="192.168.4.1"
ip=umetcp.get_wlan0_ip() # IPアドレス取得
while ip == None:
   ip=umetcp.get_wlan0_ip()
   if ip : break
   time.sleep(0.1)

umeusb.send_cmdfile("/usr/local/apps/uStartInit.umh") # ロボット初期化("R009D020010004E")
server_addr =(umetcp.get_wlan0_ip(), 59154)
hostname=socket.gethostname()
print("Serverの情報：",hostname, server_addr)

def info_show2( pitch , target_angle):
   ''' IPアドレスpとポート番号の情報と、ピッチ角度、目標角度をSSD1306ディスプレイに表示する'''
   draw_text(f"{server_addr[0]},{server_addr[1]}",0,showFlag = False, newImageFlag = True)
   draw_text(f"Pitch:{pitch:5.1f}  {target_angle}",2,showFlag = True)

# # ピッチなどの計測と、その制御ループ----------------------------------------------------------
measure_loop_flag=True # ピッチなどの計測と、その制御ループを続けるためのフラグ

def measure_loop():
   global measure_loop_flag # 測定監視ループフラグ
   next_measure_time=0 # 測定間隔制御用（次の測定の時間を記憶する）
   flag_push = False # 倒立制御開始ボタンが押されてから倒立制御を終えるまでTrue
   flag_control = False # 倒立制御中である間だけTrue
   flag_fail = False # 制御失敗か？
   start_control_time=0 # 制御開始の時間(秒)
   start_forward_time = 1.0 # 制御開始前のモータ前進を行う期間(秒)
   target_angle=23.5 # 目標角度　15→ 20→ 25← 23→
   deadband = 1.0 # デッドバンド
   time_now=0 # 現在の測定時間
   time_bak=0 # 一つ前の測定時間
   fw = None
   #
   while measure_loop_flag:
      if GPIO.input(6) == GPIO.LOW: # SW2スイッチ(緑)が押された？　リブート処理------------
         draw_text(f"Rebooting.",0,showFlag = True, newImageFlag = True)
         time.sleep(0.01)
         measure_loop_flag=False
         os.system("sudo reboot") # OSにリブートコマンドを送信
         sock.close()
         break
      #
      if GPIO.input(17) == GPIO.LOW: # SW4スイッチ(黒色)が押された？ 制御終了-----------
         flag_push = False
         flag_control = False
         if fw and fw.closed == False:
            fw.write('End\n')
            fw.close() # 保存終了
            fw = None
      #
      if next_measure_time > time.time():
         continue # 次の周期まで待つ
      time_bak=time_now
      time_now=time.time()      
      next_measure_time = time_now + 0.0002 # 次の測定時間を更新

      #
      if flag_push==False and GPIO.input(16) == GPIO.LOW: # 制御開始用SW3スイッチ(黄色)が押された？
         time.sleep(2)
         fw = open('/usr/local/apps/log.txt', 'w')
         start_control_time = time.time() # 制御スタート時間
         flag_push = True
      #
      heading, roll, pitch = read_euler() # ジャイロ測定測定
      if flag_push == False : info_show2(pitch,target_angle) # 測定値の表示
      #
      if flag_push and flag_control == False: # 倒立制御に入る前の処理
         if time.time() < start_control_time + start_forward_time: 
            umeusb.usb_send("R009D020200004D", quantity=0) # Forward 最初の助走
            #umeusb.usb_send("R009D020A00003E", quantity=0) # STOP
         else:
            umeusb.usb_send("R009D020300004C", quantity=0) # Back 最初の倒立のための逆転
            fw.write(f'B:{time_now-time_bak} pitch:{pitch}\n')    
            if pitch > target_angle: # 倒立角度に達した？
               flag_control = True
               fw.write(f'S:{time.time()} pitch:{pitch}\n')
      #
      if flag_control: # 倒立振子制御中
         if pitch > target_angle + deadband:
            umeusb.usb_send("R009D020200004D", quantity=0) # Forward
            fw.write(f'F:{time_now-time_bak} pitch:{pitch}\n')
         elif pitch < target_angle-deadband:
            umeusb.usb_send("R009D020300004C", quantity=0) # Back
            fw.write(f'B:{time_now-time_bak} pitch:{pitch}\n')
         else:       
            umeusb.usb_send("R009D020A00003E", quantity=0) # STOP
            fw.write(f'S:{time_now-time_bak} pitch:{pitch}\n')
         #
         if time.time() > start_control_time + 3: #この時間で制御を終了
            umeusb.usb_send("R009D020A00003E", quantity=0) # STOP
            flag_push = False
            flag_control = False
            if fw and fw.closed == False:
               fw.write('End\n')
               fw.close() # 保存終了
               fw = None
         #
   #


#info_show() # ディスプレイへ表示
t_id2 = threading.Thread(target=measure_loop)
t_id2.start()

# 終了時に実行したい処理 ★
def handle_shutdown(signum, frame):
   global measure_loop_flag
   if measure_loop_flag == False: return
   print(f"シャットダウンを検知しました (Signal: {signum})")
   measure_loop_flag = False
   draw_text(f"Wait....",0,showFlag = True, newImageFlag = True)
   time.sleep(10)
   draw_text(f"Shutdown.",0,showFlag = True, newImageFlag = True)
   time.sleep(0.01)

# SIGTERM（システム終了時の標準信号）を登録 ★
signal.signal(signal.SIGTERM, handle_shutdown) # ★

# TCPサーバー起動処理
try:
   serversock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
   serversock.bind(server_addr)  # IPとポート番号を指定します
   print(f"{server_addr}:接続要求を待つ")
   serversock.listen()
except : serversock = None
while serversock != None:
   print("接続を待って、接続してきたら許可")
   sock, address = serversock.accept()#サーバの接続受け入れ
   print("接続相手：",address)
   try:
      receiveData( sock ) # 受信ループ
   except Exception as e:
      print(f"例外の種類: {type(e)}")
      print(f"スタックトレース: {traceback.format_exc()}")
      draw_text(f"wait next",0,showFlag = True, newImageFlag = True)
   sock.close()
単純に「Pitch < target_angle」であれば逆転させて、「Pitch > target_angle」であれば正転させるだけの制御では、 振動が収まり難くオーバーシュートを繰り返すことが多い。
そこで、上記のコードでは deadband = 1.0 # デッドバンドの値を使って 目標角度の±deadbandの範囲ではモータを止めるようにして振動を抑える目論見のコードです。 
if pitch ＞ target_angle + deadband: 
	前進の命令をUSBでUMEHOSHI ITAに送る。
elif pitch ＜ target_angle - deadband: 
	後進の命令をUSBでUMEHOSHI ITAに送る。
else: 
	停止の命令をUSBでUMEHOSHI ITAに送る。
このような手法は、「Bang-Bang制御」と呼ばれます。
以上のコードの動作イメージを以下に示します。（下記クリックでXのムービに移動します。）

このようなバンバン制御は、制御対象の状態を目標値に保つために、 制御入力が最大値または最小値のいずれかに切り替わる制御方式で、 制御入力が急激に変化するため、制御対象の挙動が滑らかになりにくいという欠点があります。
そのせいか、目標値付近で振動的な挙動が発生して、安定しませんでした。
（）
対策として、バンバン制御に似ている方法でヒステリシスを加える方法があるそうである。
（状態遷移のしきい値の差を設けることで、頻繁な切り替えを防ぐ考え）
そのしきい値を検討するため、どのように制御しているかモータ制御のタイミングをファイル化してみました。
をグラフ化




PWM制御を検討する もしモータの回転速度を変えられるなら、角度の差に応じて出力を変えると、よりスムーズな制御ができるよ。たとえば： python error = pitch - target_angle pwm = Kp * error motor.set_pwm(pwm) ただし、これはモータがPWM制御に対応してる必要があるね。 上記のコードは、こちらのEEPROM化のためのシンプルなモータ制御コードを利用しています。
このEEPROMのエントリポイントで、PWMのデューティ比（0〜0x0FFFF)を、左右それぞれを0x1000毎の変更ができますが、大きな変動や左右同時変更ができません。