zumo32u4
#◆ 前回記事
RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_015日目_SLAM_自律走行の前準備_CartoGrapherによる自宅の地図作成 【成功!!】 の続き
#◆ はじめに
前回 は RaspberryPi(Raspbian Stretch) で Google CartoGrapher
のオドメトリを無効にしてLiDARの点群データだけで高精度の地図を生成したが、今回は将来的にナビゲーションを実施することを見据えて、真面目にオドメトリを調整しようと思う。
結果を先に公開するが、オドメトリを使用したうえでも下図のような動きへ大幅に改善した。
機体のプロットを正しく矯正できている。
突如ワープしたり、大きく明後日の方向へ移動したりはしなくなった。
角度、移動距離、共に申し分ない。
足回りのセンサー精度が若干あやしく、ほんの少しだけ歪んだ地図になってしまったが、かなりの精度で地図生成と自己位置推定を同時に実施できるようになった。
地図単体で見ると、LiDARのみで生成したときのほうが精度が高かった ため、要件によっては
(1) LiDAR単体で綺麗に地図生成
(2) LiDARとオドメトリ、IMUで位置推定
を別々の段取りで実施するのが良策かもしれない。
Google CartoGrapher の性能が良いので、自動で誤差を補正してくれる仕様にはかなり助けられている。
##● 環境
- Ubuntu 16.04 (rvizによる地図可視化用PC)
- Windows10 + TeraTerm (ロボット操作用PC)
- Zumo32u4 (ロボット本体)
- RaspberryPi3 + Raspbian Stretch (ロボット本体、CartoGrapher)
- RPLidar A1M8 (ロボット本体、測域)
- ELP-USB8MP02G-L75 (USBカメラ、動画撮影用)
#◆ 作業に掛かる前の前提条件
Raspbian Stretch+NCS(Neural Compute Stick)+YoloV2+Webカメラ+ROSによるリアルタイム複数動体検知環境の構築 と 002日目 003日目 004日目 005日目 006日目 008日目 009日目 014日目 015日目 の記事を参考に、全ての作業を実施済みであり、導入済みパッケージの種類と数の同期がとれていることを前提とする。
#◆ センサー値や物理仕様の関連性の理解
もはや、算数程度の数値計算の理解ぐらいは避けて通れないと思った。
大したことは出来ないが、メーカーの部品表や設計図を読み漁り、自分にとって分り易い簡単な絵を書いて、プログラム中に記載してある数式の意味を一つづつ紐付けしながら、チューニングすべき箇所を特定していった。
まずは、距離・スピード・回転角・機体の向き、を正しく計算するために、部品単位のサイズや、出力仕様を整理する必要がると考えたため、稚拙だが下記のようにラフに整理してみた。
モーターの減速比: 100.37:1
モーターの減速比 = モーター100.37回転でタイヤ(スプロケット)が1回転
スプロケットサイズ = 35mm
スプロケット + キャタピラ風ゴムの厚み = 39mm = タイヤの直径(自重による沈み込みは非考慮)
タイヤ1回転で進む距離 = タイヤの直径 × 3.14(円周率)
タイヤ1回転で進む距離 = 39mm × 3.14(円周率) = 122.46mm
エンコーダ分解能 = 12パルス(エンコーダ1回転)
タイヤ1回転あたりのパルス数 = 100.37 × 12 = 1204.44パルス(エンコーダカウント)
1パルスで進む距離 = 122.46mm / 1204.44パルス(エンコーダカウント) = 0.10mm
10パルスで進む距離 = 1mm
100パルスで進む距離 = 1cm(10mm)
1000パルスで進む距離 = 10cm(100mm)
左右のタイヤ間の距離:84mm (左右のタイヤを半分に輪切りにしたときの中心部同士の距離)
delta(差分時間):直近センサー値取得からの経過時間
theta(角度):回転角度
Roll(ロール):前後方向の軸に対しての回転(側転のイメージ)
Pitch(ピッチ):左右方向の軸に対しての回転(前転・後転のイメージ)
Yaw(ヨー):上下方向の軸に対しての回転(右折・左折のイメージ)
tf.transformations.quaternion_from_euler(Roll, Pitch, Yaw)
↓ Yaw方向に対する回転のみTF通知
tf.transformations.quaternion_from_euler(0, 0, self.theta)
3.14ラジアン=180°=全周の半分=0.042m×2×3.14=0.26376m
1.57ラジアン=90°=4分の1周=0.13188m
1ラジアン=0.084m
タイヤ1周=100.37×12エンコーダカウント
タイヤ外周=1204.44カウント=0.039m×3.14=0.12246m
タイヤ1周=0.12246m÷0.084m=
1.4579ラジアン
90°(4分の1周)はタイヤ何回転か=0.13188m÷0.12246m=1.0769回転
何カウントか=1.0769回転×1204.44=1,297.0614カウント
何ラジアンか=1.4579×1.0769=1.57ラジアン
180°(半周)はタイヤ何回転か=0.26376m÷0.12246m=2.1538回転
何カウントか=2.1538回転×1204.44=2,594.1229カウント
何ラジアンか=1.4579×2.1538=3.14ラジアン
ラジアンは、半径を1としたときの円周の長さを角度と言い直した単位
90°=1/4×2πラジアン
180°=1/2×2πラジアン
270°=3/4×2πラジアン
360°=1×2πラジアン
450°=5/4×2πラジアン
左右車輪が排他的に動く場合のラジアン計算
前進時の左右エンコーダ平均=左エンコーダ正、右エンコーダ正→左右の絶対値を足して左輪の符号を付与÷2
後進時の左右エンコーダ平均=左エンコーダ負、右エンコーダ負→左右の絶対値を足して左輪の符号を付与÷2
右折時の左右エンコーダ平均=左エンコーダ正、右エンコーダ負→左右の絶対値を足して左輪の符号を付与÷2
左折時の左右エンコーダ平均=左エンコーダ負、右エンコーダ正→左右の絶対値を足して左輪の符号を付与÷2
数式化
絶対値:abs(x)
((a>0)×1+(a<0)×-1)×(abs(a)+abs(b))÷2
((a>0)-(a<0))×(abs(a)+abs(b))÷2
エンコーダ1カウントあたりのラジアン=3.14÷2,594.1229=0.001210428388=0.00121043
回転角(ラジアン)=((a>0)-(a<0))×(abs(a)+abs(b))÷2×0.00121043
Pythonのロジック上の定数や数式には大きな問題となりそうな箇所は見当たらなかったが、調べを進めて新たに理解したことは下記。
1. Zumo32u4に付属するエンコーダは 符号付きInt16 で値を保持する
2. 符号付きInt16 の最大値・最小値は 32,767 〜 -32,768
3. 機体を前進するとエンコーダ値は 0 から 1ずつ増えて、32,767 まで到達すると、オーバーフローして突然 -32,768 にワープする → 地図上で突如ワープして迷子になっていた原因
4. エンコーダのカウントがオーバーフローして突如ワープすることにより、回転角の計算や距離の計算がブッ飛んだ値になる
5. かたや、Python2.7 のfloat型はかなりの範囲の有効桁数を取りうる
ということで、対策を講ずる一筋の光がチラリと見えた気がした。
#◆ オドメトリの調整
(1) 限界ギリギリまで各種変数がオーバーフローしないように仕様を見直す
(2) オーバーフローする前提の仕様へ変更する
のいずれかで本質的な対策ができそうだ。
小型ロボットごときで短時間では使い切れないほどの幅を持つPythonのfloat型のレンジを有効活用すれば、実用に十分耐えられる仕様になる、と判断し、(1)を採用することにした。
1.Arduinoのエンコーダから、積算値
ではなく ある断面の瞬発的な値
を取るように変更する。
newLeft = encoders.getCountsAndResetLeft();
newRight = encoders.getCountsAndResetRight();
2.RaspberryPi側のPythonでは、 エンコーダから 積算値
を受け取ることを期待するのではなく、 ある断面の瞬発的な値
を連続で受信し、自分でfloat型へ積算する。
VR=(float(self.sensorvalue[10])-float(self.odomR))/self.COUNT*3.14*self.DIAMETER/self.delta
VL=(float(self.sensorvalue[9])-float(self.odomL))/self.COUNT*3.14*self.DIAMETER/self.delta
self.o.pose.pose.position.x += self.delta*(VR+VL)/2*cos(self.theta)
self.o.pose.pose.position.y += self.delta*(VR+VL)/2*sin(self.theta)
rad = 0.0
if (self.odomL>0.0 and self.odomR<0.0) or (self.odomL<0.0 and self.odomR>0.0):
rad = ((self.odomL>0)-(self.odomL<0))*(abs(self.odomL)+abs(self.odomR))/2*self.RADIANPERENCODER
self.theta += rad
3.Google CartoGrapher
で オドメトリ
と IMU
の使用を有効化する。
use_odometry = true,
TRAJECTORY_BUILDER_2D.use_imu_data = true
#◆ 修正を加えた各ファイル
**zumo32u4.py の内容**
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys
import traceback
import serial
from math import sqrt, cos, sin
import rospy
import tf
from time import sleep
from geometry_msgs.msg import Twist, Pose
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from std_msgs.msg import String
class Zumo:
def __init__(self):
self.DIAMETER=0.039 #[Meter] Diameter of tire
self.INTERAXIS=0.084 #[Meter] Distance between left and right tires
self.COUNT=12 #[Count] Resolution of encoder
self.RADIANPERENCODER=0.00121043 #[Radian] Radian per encoder count 1
self.temps=0.0 #[Second] Last measured time
self.delta=0.0 #[Second] Elapsed time
self.theta=3.14 #[Radian] Radian
self.command="" #[String] Robot operation command
self.odomL=0
self.odomR=0
try:
self.PORT = rospy.get_param('BLUETOOTH_PORT')
except:
rospy.set_param('BLUETOOTH_PORT',"/dev/rfcomm0")
self.PORT = rospy.get_param('BLUETOOTH_PORT')
try:
self.BAUDRATE = rospy.get_param('BLUETOOTH_BAUDRATE')
except:
rospy.set_param('BLUETOOTH_BAUDRATE', "115200")
self.BAUDRATE = rospy.get_param('BLUETOOTH_BAUDRATE')
self.TIMEOUT = 0.01
self.sensorvalue = list()
self.speed = []
self.angle = []
# Init Odometry
self.o = Odometry()
self.o.pose.pose.position.x = 0.0
self.o.pose.pose.position.y = 0.0
self.o.pose.pose.position.z = 0.0
self.o.pose.pose.orientation.z = 3.14
self.o.header.stamp = rospy.Time.now()
self.o.header.frame_id = "odom"
self.o.child_frame_id = "base_link"
# Init IMU
self.p = Imu()
self.p.header.stamp = rospy.Time.now()
self.p.header.frame_id = "imu_link"
try:
self.ser = serial.Serial(self.PORT, self.BAUDRATE, timeout = self.TIMEOUT)
sleep(1)
rospy.loginfo("Serial connection established on the port "+str(self.PORT))
except:
rospy.logwarn("Serial connection failure")
self.pub_comm = rospy.Publisher('command', String, queue_size=10)
rospy.loginfo("Publisher initialization success /command")
self.pub_imu = rospy.Publisher('imu', Imu, queue_size=10)
rospy.loginfo("Publisher initialization success /imu")
self.pub_odom = rospy.Publisher('odom', Odometry, queue_size=10)
rospy.loginfo("Publisher initialization success /odom")
self.sub_sensorval = rospy.Subscriber('sensorval', String, self.subsensorval)
rospy.loginfo("Subscriber initialization success /sensorval")
self.tf_br = tf.TransformBroadcaster()
rospy.loginfo("TransformBroadcaster initialization success")
def __delete__(self):
self.ser.close()
def pubcommand(self):
try:
self.ser.flush()
self.command = ""
self.command = self.ser.read()
if self.command != "":
rospy.loginfo("Command received ["+self.command+"]")
self.pub_comm.publish(self.command)
except:
pass
def subsensorval(self, svalue):
try:
if len(svalue.data) > 0:
#rospy.loginfo(svalue.data)
self.sensorvalue = svalue.data.split(',')
if len(self.sensorvalue) == 14:
self.pubimu()
self.pubodom()
except:
print "subsensorval Error"
traceback.print_exc()
def pubimu(self):
self.p.linear_acceleration.x=4*9.81*(float(self.sensorvalue[1])/2**16)/100
self.p.linear_acceleration.y=4*9.81*(float(self.sensorvalue[2])/2**16)/100
self.p.linear_acceleration.z=4*9.81*(float(self.sensorvalue[3])/2**16)/100
self.p.orientation.x= float(self.sensorvalue[4])
self.p.orientation.y=float(self.sensorvalue[5])
self.p.orientation.z=float(self.sensorvalue[6])
self.p.header.stamp = rospy.Time.now()
self.pub_imu.publish(self.p)
def pubodom(self):
VL=0.0
VR=0.0
if float(self.sensorvalue[10])!=self.odomR or float(self.sensorvalue[9])!=self.odomL:
self.delta=(float(self.sensorvalue[0])-float(self.temps))/1000 #[Second] Elapsed time from latest measurement
VR=(float(self.sensorvalue[10])-float(self.odomR))/self.COUNT*3.14*self.DIAMETER/self.delta #[Meter] Advance distance of right wheel
VL=(float(self.sensorvalue[9])-float(self.odomL))/self.COUNT*3.14*self.DIAMETER/self.delta #[Meter] Advance distance of left wheel
self.odomL=float(self.sensorvalue[9])
self.odomR=float(self.sensorvalue[10])
self.temps=self.sensorvalue[0]
#rospy.loginfo("[odomL] "+str(self.odomL)+" [odomR] "+str(self.odomR)+" [delta] "+str(self.delta)+" [VL] "+str(VL)+" [VR] "+str(VR))
else :
VR=0.0
VL=0.0
self.temps=self.sensorvalue[0]
self.o.pose.pose.position.x += self.delta*(VR+VL)/2*cos(self.theta)
self.o.pose.pose.position.y += self.delta*(VR+VL)/2*sin(self.theta)
rad = 0.0
if (self.odomL>0.0 and self.odomR<0.0) or (self.odomL<0.0 and self.odomR>0.0):
rad = ((self.odomL>0)-(self.odomL<0))*(abs(self.odomL)+abs(self.odomR))/2*self.RADIANPERENCODER
self.theta += rad
quat = tf.transformations.quaternion_from_euler(0,0,self.theta)
self.o.pose.pose.orientation.x = quat[0]
self.o.pose.pose.orientation.y = quat[1]
self.o.pose.pose.orientation.z = quat[2]
self.o.pose.pose.orientation.w = quat[3]
self.o.twist.twist.linear.x =(VR+VL)/2*cos(self.theta)
self.o.twist.twist.linear.y =(VR+VL)/2*sin(self.theta)
self.o.twist.twist.angular.z = rad
self.o.header.stamp = rospy.Time.now()
self.pub_odom.publish(self.o)
pos = (self.o.pose.pose.position.x,
self.o.pose.pose.position.y,
self.o.pose.pose.position.z)
ori = (self.o.pose.pose.orientation.x,
self.o.pose.pose.orientation.y,
self.o.pose.pose.orientation.z,
self.o.pose.pose.orientation.w)
self.tf_br.sendTransform(pos, ori, rospy.Time.now(), 'base_link', 'odom')
if __name__=="__main__":
rospy.init_node("zumo", anonymous = True)
zumo = Zumo()
while not rospy.is_shutdown():
zumo.pubcommand()
sleep(0.001)
rospy.delete_param("BLUETOOTH_PORT")
rospy.delete_param("BLUETOOTH_BAUDRATE")
zumo.ser.close()
**zumo32u4arduino.ino の内容**
#define USE_USBCON
#include <ros.h>
#include <std_msgs/String.h>
#include <Wire.h>
#include <LSM303.h>
#include <Zumo32U4.h>
long timer=0; // Elapsed time since program started (milli second)
int vright = 0; // Left Morter velocity (speed of motor)
int vleft = 0; // Right Morter velocity (speed of motor)
int basespeed = 150; // Base speed of Morter (Effective Range: 1 - 150)
int16_t positionLeft = 0; // For encoder verification
int16_t positionRight = 0; // For encoder verification
int16_t newLeft = 0; // Value of Encorder
int16_t newRight = 0; // Value of Encorder
std_msgs::String str_msg; // Sensor value to be published
LSM303 compass; // Magnetometer
L3G gyro; // Gyrometer
Zumo32U4Motors motors; // Morter
Zumo32U4Encoders encoders; // Encoder
ros::NodeHandle nh; // NodeHandler of ROS
void motorcontrol(const std_msgs::String& cmd_msg)
{
// I : forward , , : backward
// L : turn right , J : turn left
// S : stop
String cmd = "";
//Serial.println(cmd_msg.data); // Debug Print
if (strlen(cmd_msg.data) != 0)
{
cmd = cmd_msg.data;
}
if (cmd == "i")
{
motors.setSpeeds(0, 0);
delay(2);
vleft = basespeed;
vright = basespeed;
motors.setSpeeds(vleft, vright);
}
else if (cmd == ",")
{
motors.setSpeeds(0, 0);
delay(2);
vleft = -1*basespeed;
vright = -1*basespeed;
motors.setSpeeds(vleft, vright);
}
else if (cmd == "l")
{
motors.setSpeeds(0, 0);
delay(2);
vleft = (basespeed+230);
vright = -1*(basespeed+230);
motors.setSpeeds(vleft, vright);
}
else if (cmd == "j")
{
motors.setSpeeds(0, 0);
delay(2);
vleft = -1*(basespeed+230);
vright = (basespeed+230);
motors.setSpeeds(vleft, vright);
}
else if (cmd == "s")
{
vleft = 0;
vright = 0;
motors.setSpeeds(vleft, vright);
delay(2);
}
}
ros::Subscriber<std_msgs::String> sub("/command", motorcontrol);
ros::Publisher chatter("/sensorval", &str_msg);
void setup()
{
//Serial.begin(9600); // Debug Print
Wire.begin();
encoders.getCountsAndResetLeft();
encoders.getCountsAndResetRight();
nh.initNode(); // Init ROS Node
nh.advertise(chatter); // Init ROS Publisher
nh.subscribe(sub); // Init ROS Subscriber
compass.init(); // Init magnetometer
compass.enableDefault();
gyro.init(); // Init gyrometer
gyro.enableDefault();
}
void loop()
{
compass.read(); // Read magnetometer
gyro.read(); // Read gyrometer
timer = millis();
newLeft = encoders.getCountsAndResetLeft();
newRight = encoders.getCountsAndResetRight();
if (!(encoders.checkErrorLeft()) && !(encoders.checkErrorRight())) {
positionLeft = newLeft;
positionRight = newRight;
}
String s = "";
s += timer; // [0] Elapsed time since program started (milli second)
s += ',';
s += compass.a.x; // [1] Accelerometer.x
s += ',';
s += compass.a.y; // [2] Accelerometer.y
s += ',';
s += compass.a.z; // [3] Accelerometer.z
s += ',';
s += compass.m.x; // [4] Magnetometer.x
s += ',';
s += compass.m.y; // [5] Magnetometer.y
s += ',';
s += compass.m.z; // [6] Magnetometer.z
s += ',';
s += vleft; // [7] Left Morter velocity (speed of motor)
s += ',';
s += vright; // [8] Right Morter velocity (speed of motor)
s += ',';
s += positionLeft; // [9] Left Morter odometry (Rotation angle of motor)
s += ',';
s += positionRight; // [10] Right Morter odometry (Rotation angle of motor)
s += ',';
s += gyro.g.x; // [11] Gyrometer.x
s += ',';
s += gyro.g.y; // [12] Gyrometer.y
s += ',';
s += gyro.g.z; // [13] Gyrometer.z
//Serial.println(s); // Debug Print
str_msg.data = s.c_str();
chatter.publish(&str_msg);
nh.spinOnce();
delay(1);
}
**backpack_2d.lua の内容**
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = false,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.min_range = 0.
TRAJECTORY_BUILDER_2D.max_range = 20.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7
POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight = 1e5
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight = 1e5
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e5
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e5
POSE_GRAPH.optimization_problem.huber_scale = 1e3
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.2)
return options
**backpack_2d.launch の内容**
<?xml version="1.0" ?>
<launch>
<param
name="robot_description"
textfile="$(find zumo32u4)/urdf/zumo32u4.urdf"
/>
<node
name="horizontal_laser"
pkg="rplidar_ros"
type="rplidarNode"
output="screen">
<param name="serial_port" type="string" value="/dev/ttyUSB0"/>
<param name="serial_baudrate" type="int" value="115200"/>
<param name="frame_id" type="string" value="horizontal_laser_link"/>
<param name="inverted" type="bool" value="false"/>
<param name="angle_compensate" type="bool" value="true"/>
</node>
<node
pkg="tf"
type="static_transform_publisher"
name="base_link_connect"
args="0 0 0 0 0 0 /base_link /horizontal_laser_link 100"
/>
<node
pkg="tf"
type="static_transform_publisher"
name="imu_link_connect"
args="0 0 0 0 0 0 /base_link /imu_link 100"
/>
<node
name="cartographer_node"
pkg="cartographer_ros"
type="cartographer_node"
args="-configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename backpack_2d.lua"
output="screen">
</node>
<node
name="cartographer_occupancy_grid_node"
pkg="cartographer_ros"
type="cartographer_occupancy_grid_node"
args="-resolution 0.05"
/>
</launch>
<現状のTFツリー>
#◆ 本日のまとめ
- Github を随時更新中。
- オドメトリの調整に成功。
- オドメトリを使用することで機体の位置推定の精度が向上した。
- 足回りのセンサが若干ショボいのでこれぐらいの精度が限界かな。
- RaspberryPi、酷使しすぎてモノが完成する前に壊れないことだけを願う。
- Roll、Pitch、Yaw 初代エースコンバット の時代からやり込み続け、意味を理解しないまま今日に至る。
- 久々にエースコンバットがプレイしたくなってきた。
- 今度は エースコンバット2のこの曲 RISING HIGH がグルグルし始めた。
#◆ 次回予告
ついに、 ナビゲーション
だろうか。
相当ハマりそうだ。。。