Wheel-moving robots (opposite two-wheel type) equipped with Microbit and Arduino are on sale as programming materials for children. (For example, a servo car kit using a 360-degree servo motor Move a 360-degree servo motor with Microbit) Sample programs are provided as teaching materials, such as various movements of the robot, wireless operation, and autonomous operation with sensors attached. Taking this one step further, there is something that connects cyber space (digital space) and physical space (real space) (digital twin) using data, such as simulating this movement in advance and reproducing the trajectory from the data actually moved. I thought it would be interesting. Therefore, I made a simulation that reproduces the trajectory from the information of the motor ON / OFF of the mobile robot. Retrieving ON / OFF information for the motor (or servo motor) is a future task, and that part is not included this time.
For wheel moving robots, the following site will be helpful, but the input is the speed of the left and right wheels, and the value to be set is the length between the wheels. The formula is as follows.
v: velocity \\
\omega: angular \ velocity \\
x, y: position\\
\theta: directon \ angle\\
t_d: tread
v _k = (v_r +v_l)/2 \\
\omega _k = (v_r +v_l)/t_d \\
x_{k+1} = x_k + \cos \theta _k\cdot\Delta K v_k \\
y_{k+1} = y_k + \sin \theta _k\cdot\Delta K v_k \\
\theta _{k+1} = \theta _k + \Delta K \omega _k \\
Figure: Coordinates and parameters of the wheel-moving robot (partially changed the figure in the referenced article)
Reference: Wheel moving robot principle [Moving Mecha / Robot and Basics of Control-Tohoku Gakuin University Information Processing ...](http://www.mech.tohoku-gakuin.ac.jp/rde/contents/sendai/mechatro/archive/RM Seminar_No20_s8.pdf ) Let's calculate the movement of a wheel robot (opposed two-wheel type)
Referenced code Two wheel motion model sample
In this simulation, the speeds of the left and right wheels are constant and only ON / OFF information is provided.
Move | Right wheel | Left wheel |
---|---|---|
Straight line | Previous ON | Previous ON |
Backward | After ON | After ON |
Turn right | OFF | Previous ON |
Turn left | Previous ON | OFF |
Rotate on the spot | Before ON (after ON) | After ON (before ON) |
Win 10 Pro 64bit Anaconda python 3.7
"""
Opposing two-wheeled robot
Trajectory simulation
Input parameters
Length between two wheels: tread[m]
Wheel speed Right, Left: vr, vl [m/s]
output
Vehicle center of gravity speed: ver
Turning angular velocity: omega[rad/s]
Final output
Azimuth: thetat
x,y coordinate: xt ,yt
Referenced code
Two wheel motion model sample
https://www.eureka-moments-blog.com/entry/2020/04/05/180844#%E5%AF%BE%E5%90%912%E8%BC%AA%E5%9E%8B%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88
change point:
Expand matrix format into ordinary expressions
Change from angular velocity of wheels to speed of wheels
Change simulation input
animation
Create graph (Matplotlib) animation with Python (Artist Animation)
https://water2litter.net/rum/post/python_matplotlib_animation_ArtistAnimation/
[Scientific / technical calculation by Python]Trajectory of parabolic movement animation(locus)Draw with, matplotlib
https://qiita.com/sci_Haru/items/278b6a50c4e9f4c07dcf
"""
import numpy as np
from math import cos, sin, pi
import math
import matplotlib.pyplot as plt
from matplotlib.animation import ArtistAnimation
def twe_wheel_fuc(v, state, delta, factor=1, td=2):
"""
Equation of state
Args:
v (tuple or list): velocity of each wheel unit m/s,(right velocity,left velocity)
state (list): state [x, y, thita] , x, y
delta (float): update time unit s
factor (float):velocity factor Defaults to 1
td (float): tread length between wheel unit m Defaults to 2.
Returns:
[list]: next state
"""
# vr: right wheel velocity, vl:left wheel velocity
# vel: Center of gravity velocity
# omega: Rotation angular velocity
vr = v[0]*factor
vl = v[1]*factor
vel = (vr + vl)/2
omega = (vr - vl)/(td)
# state[2]: theta
x_ = vel*delta*cos(state[2]+omega*delta/2)
y_ = vel*delta*sin(state[2]+omega*delta/2)
# x_ = vel*delta*cos(state[2])
# y_ = vel*delta*sin(state[2])
xt = state[0] + x_
yt = state[1] + y_
thetat = state[2]+omega*delta
update_state = [xt, yt, thetat]
return update_state
def simulation_twewheel(data,ini_state=[0,0,0],factor=1,td=6.36):
"""
data: list On/OFF data
"""
# simulation
#For drawing animation graphs
fig = plt.figure()
ims = []
#Storage of calculated data (coordinates)
st_x = []
st_y = []
st_theta = []
st_vec = ini_state
for i in data:
st_vec = twe_wheel_fuc(i, st_vec, delta=1,factor=factor,td=td)
xt, yt, thetat = st_vec
print("State:",st_vec)
print("Direction angle: ",math.degrees(thetat))
st_x.append(xt)
st_y.append(yt)
st_theta.append(thetat)
#Settings for Plot
plt.grid(True)
plt.axis("equal")
plt.xlabel("X")
plt.ylabel("Y")
#If only the position at time t
# im=plt.plot(xt,yt,'o', color='red',markersize=10, linewidth = 2)
#Create two pictures, the position at time t and the trajectory leading up to time t
plt.annotate('', xy=(xt+cos(thetat),yt+sin(thetat)), xytext=(xt,yt),
arrowprops=dict(shrink=0, width=1, headwidth=2,
headlength=10, connectionstyle='arc3',
facecolor='blue', edgecolor='blue'))
im=plt.plot(xt,yt,'o',st_x,st_y, '--', color='red',markersize=10, linewidth = 2)
ims.append(im)
#Animation creation
anim = ArtistAnimation(fig, ims, interval=100, blit=True,repeat=False)
plt.show()
# plt.pause(10)
if __name__ == '__main__':
#Velocity data per second
#Switch on/Set to OFF and keep the speed constant. Forward rotation: 1, reverse rotation:-1, stop: 0
#(1,1): Forward, (0,1): Clockwise, (1,0): Counterclockwise,(-1,1) or (1,-1): In-situ rotation
input_lists =[(1,1),(1,1),(1,1),(1,1),(1,1),(1,1),
(0,1),(0,1),(0,1),(0,1),(0,1),(0,1),
(1,1),(1,1),(1,1),(1,1),(1,1),(1,1),
(1,0),(1,0),(1,0),(1,0),(1,0),(1,0),
(1,1),(1,1),(1,1),(1,1),(1,1),(1,1),
(1,-1),(1,-1),(1,1),(1,1),
(1,-1),(1,-1),(1,1),(1,1),
(1,0),(1,0),(1,0),(0,1),(0,1),(0,1),
(1,0),(1,0),(1,0),(0,1),(0,1),(0,1),
(1,1),(1,1),]
input_lists2 =[(1,1),(1,1),(1,1),(1,1),(1,1),(1,1),]
simulation_twewheel(data=input_lists,ini_state=[0,0,0],factor=1,td=6.36)
We received advice from Kioto about the program code. thank you so much.
Result when input_list1 is read as data
Digital twins (cyberspace and physical space) can be connected via data. Data from physical space is the bridge, and in cyberspace, simulation by data or data by simulation is the bridge. I hope to expand from programming education to data utilization education.