プログラミング/IoT の関連商品 (Amazonのアソシエイトとして、当メディアは適格販売により収入を得ています。)
最近の投稿
概要
ロボットアプリケーションを開発する際に、ロボットの自己位置を推定する必要がある場合を考えます。ここでは Open Dynamics Engine を Python から利用した場合について、自己位置推定のサンプルを記載します。自己位置推定と環境の地図作成を同時に行う場合を SLAM (Simultaneous Localization and Mapping) とよびます。
検証に用いる車輪型ロボット
こちらのページでインストールした ODE を用いて、簡単な車輪型ロボットを作ります。ヒンジジョイントに内臓されているモータを速度制御することで移動します。
参考書籍: 『簡単!実践!ロボットシミュレーション - Open Dynamics Engineによるロボットプログラミング』
#!/usr/bin/python
# -*- coding: utf-8 -*-
import odepy
import drawstuffpy as ds
import ctypes
import numpy as np
def Main():
world, space, ground, contactgroup = CreateWorld()
# 車輪型ロボット
robot = Robot(world, space)
def __NearCallback(data, o1, o2):
o1IsGround = ctypes.addressof(ground.contents) == ctypes.addressof(o1.contents)
o2IsGround = ctypes.addressof(ground.contents) == ctypes.addressof(o2.contents)
if not (o1IsGround or o2IsGround):
return
N = 10
contacts = (odepy.dContact * N)()
n = odepy.dCollide(o1, o2, N, ctypes.byref(contacts[0].geom), ctypes.sizeof(odepy.dContact))
for i in range(n):
contact = contacts[i]
contact.surface.mu = 99 # ある程度小さな値にする必要があります。
contact.surface.mode = odepy.dContactBounce
contact.surface.bounce = 1.0
contact.surface.bounce_vel = 0.0
c = odepy.dJointCreateContact(world, contactgroup, ctypes.byref(contact))
odepy.dJointAttach(c, odepy.dGeomGetBody(contact.geom.g1), odepy.dGeomGetBody(contact.geom.g2))
def __StepCallback(pause):
# ロボットの速度制御
robot.Control()
# 現在座標を表示
x, y = robot.GetPos()
print('{}\t{}'.format(x, y))
odepy.dSpaceCollide(space, 0, odepy.dNearCallback(__NearCallback))
odepy.dWorldStep(world, 0.01)
odepy.dJointGroupEmpty(contactgroup)
for geom in robot.GetGeoms():
if odepy.dGeomGetClass(geom) == odepy.dBoxClass:
ds.dsSetColor(1.0, 1.0, 0.0)
lengths = odepy.dVector3()
odepy.dGeomBoxGetLengths(geom, lengths)
ds.dsDrawBoxD(odepy.dGeomGetPosition(geom), odepy.dGeomGetRotation(geom), lengths)
if odepy.dGeomGetClass(geom) == odepy.dCylinderClass:
ds.dsSetColor(1.0, 1.0, 1.0)
r = odepy.dReal()
l = odepy.dReal()
odepy.dGeomCylinderGetParams(geom, ctypes.byref(r), ctypes.byref(l))
ds.dsDrawCylinderD(odepy.dGeomGetPosition(geom), odepy.dGeomGetRotation(geom), l.value, r.value)
def __Command(cmd):
if chr(cmd) == 'l':
robot.SetVelL(robot.GetVelL() + 1.0)
if chr(cmd) == 'L':
robot.SetVelL(robot.GetVelL() - 1.0)
if chr(cmd) == 'r':
robot.SetVelR(robot.GetVelR() + 1.0)
if chr(cmd) == 'R':
robot.SetVelR(robot.GetVelR() - 1.0)
if chr(cmd) == 's':
robot.SetVelL(0.0)
robot.SetVelR(0.0)
RunDrawStuff(__StepCallback, __Command)
DestroyWorld(world, space)
class Robot(object):
def __init__(self, world, space, lx=0.4, ly=0.4, lz=0.2, rLR=0.11, rFB=0.05, w=0.02, mBase=9.4, mLR=0.2, mFB=0.1, px=0.0, py=0.0, pz=0.2):
self.__velL = 0.0
self.__velR = 0.0
self.__base = self.__AddBox(world, space, lx=lx, ly=ly, lz=lz, m=mBase, px=px, py=py, pz=pz)
self.__wheelL = self.__AddCylinder(world, space, r=rLR, l=w, m=mLR, px=-(lx + w) / 2.0, py=0.0, pz=(pz - lz / 2.0))
self.__wheelR = self.__AddCylinder(world, space, r=rLR, l=w, m=mLR, px=(lx + w) / 2.0, py=0.0, pz=(pz - lz / 2.0))
self.__wheelF = self.__AddCylinder(world, space, r=rFB, l=w, m=mFB, px=0.0, py=(ly / 2.0 - rFB), pz=(pz - lz / 2.0 - rFB))
self.__wheelB = self.__AddCylinder(world, space, r=rFB, l=w, m=mFB, px=0.0, py=-(ly / 2.0 - rFB), pz=(pz - lz / 2.0 - rFB))
self.__jointL = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelL), odepy.dGeomGetPosition(self.__wheelL))
self.__jointR = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelR), odepy.dGeomGetPosition(self.__wheelR))
self.__jointF = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelF), odepy.dGeomGetPosition(self.__wheelF))
self.__jointB = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelB), odepy.dGeomGetPosition(self.__wheelB))
def GetVelL(self):
return self.__velL
def GetVelR(self):
return self.__velR
def SetVelL(self, velL):
self.__velL = velL
def SetVelR(self, velR):
self.__velR = velR
def GetPos(self):
pos = odepy.dGeomGetPosition(self.__base)
return pos[0], pos[1]
def Control(self):
fMax = 100
odepy.dJointSetHingeParam(self.__jointL, odepy.dParamVel, self.__velL)
odepy.dJointSetHingeParam(self.__jointL, odepy.dParamFMax, fMax)
odepy.dJointSetHingeParam(self.__jointR, odepy.dParamVel, self.__velR)
odepy.dJointSetHingeParam(self.__jointR, odepy.dParamFMax, fMax)
def GetGeoms(self):
return self.__base, self.__wheelL, self.__wheelR, self.__wheelF, self.__wheelB
def __AddBox(self, world, space, lx, ly, lz, m, px, py, pz):
geom = odepy.dCreateBox(space, lx, ly, lz)
body = odepy.dBodyCreate(world)
mass = odepy.dMass()
odepy.dMassSetZero(ctypes.byref(mass))
odepy.dMassSetBoxTotal(ctypes.byref(mass), m, lx, ly, lz)
odepy.dGeomSetBody(geom, body)
odepy.dBodySetMass(body, ctypes.byref(mass))
odepy.dBodySetPosition(body, px, py, pz)
q = odepy.dQuaternion()
odepy.dQFromAxisAndAngle(q, 0, 0, 1, 0.0)
odepy.dBodySetQuaternion(body, q)
return geom
def __AddCylinder(self, world, space, r, l, m, px, py, pz, direction=3):
geom = odepy.dCreateCylinder(space, r, l)
body = odepy.dBodyCreate(world)
mass = odepy.dMass()
odepy.dMassSetZero(ctypes.byref(mass))
odepy.dMassSetCylinderTotal(ctypes.byref(mass), m, direction, r, l)
odepy.dGeomSetBody(geom, body)
odepy.dBodySetMass(body, ctypes.byref(mass))
odepy.dBodySetPosition(body, px, py, pz)
q = odepy.dQuaternion()
odepy.dQFromAxisAndAngle(q, 0, 1, 0, np.pi / 2.0)
odepy.dBodySetQuaternion(body, q)
return geom
def __AddJoint(self, world, body1, body2, pos):
joint = odepy.dJointCreateHinge(world, 0)
odepy.dJointAttach(joint, body1, body2)
odepy.dJointSetHingeAxis(joint, 1, 0, 0)
odepy.dJointSetHingeAnchor(joint, pos[0], pos[1], pos[2])
return joint
def RunDrawStuff(stepCallback, command, pathToTextures='/usr/local/share/ode-0.16.1-drawstuff-textures', w=400, h=225, cameraXyz=[3.0, 0.0, 1.0], cameraHpr=[-180.0, 0.0, 0.0]):
def __StartCallback():
xyz = (ctypes.c_float * 3)()
hpr = (ctypes.c_float * 3)()
for i, v in enumerate(cameraXyz):
xyz[i] = v
for i, v in enumerate(cameraHpr):
hpr[i] = v
ds.dsSetViewpoint(xyz, hpr)
fn = ds.dsFunctions()
fn.version = ds.DS_VERSION
fn.start = ds.dsStartCallback(__StartCallback)
fn.step = ds.dsStepCallback(stepCallback)
fn.path_to_textures = ctypes.create_string_buffer(pathToTextures.encode('utf-8'))
fn.command = ds.dsCommandCallback(command)
ds.dsSimulationLoop(0, ctypes.byref(ctypes.POINTER(ctypes.c_char)()), w, h, fn)
def CreateWorld():
odepy.dInitODE()
world = odepy.dWorldCreate()
odepy.dWorldSetGravity(world, 0.0, 0.0, -9.8)
space = odepy.dHashSpaceCreate(0)
ground = odepy.dCreatePlane(space, 0, 0, 1, 0)
contactgroup = odepy.dJointGroupCreate(0)
return world, space, ground, contactgroup
def DestroyWorld(world, space):
odepy.dSpaceDestroy(space)
odepy.dWorldDestroy(world)
odepy.dCloseODE()
if __name__ == '__main__':
Main()
外界センサを使う場合
距離情報を用いる方法
三つのランドマークの位置が既知であるとします。距離センサで取得したランドマークからの距離をもとに自己位置を推定することができます。
def UseDistInfo(x, y, eps=1e-10):
# ランドマークの位置は既知であるとします。
x1, y1 = 10.0, 10.0
x2, y2 = 0.0, -10.0
x3, y3 = -10.0, 10.0
# 距離センサを用いてランドマークからの距離が得られた状況を考えます。
p = np.array([x, y])
p1 = np.array([x1, y1])
p2 = np.array([x2, y2])
p3 = np.array([x3, y3])
r1 = np.linalg.norm(p1 - p)
r2 = np.linalg.norm(p2 - p)
r3 = np.linalg.norm(p3 - p)
# ランドマークの位置、ランドマークからの距離 を用いて現在位置を推定できます。
D = (x1 - x2) * (y2 - y3) - (y1 - y2) * (x2 - x3)
a = np.array([
[y2 - y3, -y1 + y2],
[-x2 + x3, x1 - x2]])
b = np.array([
[x1 ** 2 - x2 ** 2 + y1 ** 2 -y2 ** 2 + r2 ** 2 - r1 ** 2],
[x2 ** 2 - x3 ** 2 + y2 ** 2 -y3 ** 2 + r3 ** 2 - r2 ** 2]])
res = a.dot(b) / (2 * D)
# 推定結果を検証します。
assert(abs(x - res[0]) < eps)
assert(abs(y - res[1]) < eps)
def __StepCallback(pause):
# ロボットの速度制御
robot.Control()
# 現在座標を表示
x, y = robot.GetPos()
# print('{}\t{}'.format(x, y))
UseDistInfo(x, y)
# ...以下略
角度情報を用いる方法
三つのランドマークの位置が既知であるとします。ロボットの進行方向からランドマークまでの角度をもとに自己位置を推定できます。角度情報は、例えば全方位カメラからの画像を処理することで取得できます。ランドマークは、以下の beta の値が 360 度の範囲で均等に配置されているようなものを選ぶと推定精度が高くなります。
def UseAngleInfo(x, y, eps=1e-10):
# ランドマークの位置は既知であるとします。
x1, y1 = 10.0, 10.0
x2, y2 = 0.0, -10.0
x3, y3 = -10.0, 10.0
# 進行方向の角度は任意の値でよいです。仮に以下の値であるとします。
phi = np.pi / 4.0
# センサ情報によって、「進行方向」と「ランドマークのある方向」の角度差が得られた状況を考えます。
beta1 = np.arctan((x1 - x) / (y1 - y)) - phi
beta2 = np.arctan((x2 - x) / (y2 - y)) - phi
beta3 = np.arctan((x3 - x) / (y3 - y)) - phi
# ランドマークの位置、進行方向からランドマークまでの角度 を用いて現在位置を推定できます。
d12 = np.tan(beta2 - beta1)
d23 = np.tan(beta3 - beta2)
d31 = np.tan(beta1 - beta3)
a12 = x1 + x2 + (y2 - y1) / d12
a23 = x2 + x3 + (y3 - y2) / d23
a31 = x3 + x1 + (y1 - y3) / d31
b12 = y1 + y2 - (x2 - x1) / d12
b23 = y2 + y3 - (x3 - x2) / d23
b31 = y3 + y1 - (x1 - x3) / d31
c12 = x1 * x2 + y1 * y2 - (x2 * y1 - x1 * y2) / d12
c23 = x2 * x3 + y2 * y3 - (x3 * y2 - x2 * y3) / d23
c31 = x3 * x1 + y3 * y1 - (x1 * y3 - x3 * y1) / d31
D = (a23 - a12) * (b31 - b23) - (b23 - b12) * (a31 - a23)
xx = ((b31 - b23) * (c23 - c12) - (b23 - b12) * (c31 - c23)) / D
yy = ((a31 - a23) * (c23 - c12) - (a23 - a12) * (c31 - c23)) / -D
# 推定結果を検証します。
assert(abs(x - xx) < eps)
assert(abs(y - yy) < eps)
def __StepCallback(pause):
# ロボットの速度制御
robot.Control()
# 現在座標を表示
x, y = robot.GetPos()
# print('{}\t{}'.format(x, y))
UseAngleInfo(x, y)
# ...以下略
内界センサを使う場合
車輪の回転速度を時間積分していくことでロボットの現在位置と角度を推定することができます。内界センサを利用して推定する方法をデッドレコニング (Dead Reckoning) とよびます。マイクロマウス等で利用される加速度センサ、ジャイロ、ロータリーエンコーダは内界センサです。車輪と地面の間のすべり等が存在するため誤差が蓄積されていきます。定期的な補正が必要です。
ロボット重心の座標を $x$、$y$、進行方向の角度を $\phi$ とすると、車輪の回転速度 $\omega$ を用いて以下の式が成り立ちます。車輪が地面と接触している箇所における速度は $r \omega$ です。$d$ は車体重心から車輪までの距離、$r$ は車輪の半径です。
$$\begin{eqnarray} \frac{d \phi(t)}{d t} &=& \frac{r \omega_l(t) - r \omega_r(t)}{2 d} \\ \frac{d x(t)}{d t} &=& \frac{r \omega_l(t) \sin{\phi(t)} + r \omega_r(t) \sin{\phi(t)}}{2} \\ \frac{d y(t)}{d t} &=& \frac{r \omega_l(t) \cos{\phi(t)} + r \omega_r(t) \cos{\phi(t)}}{2} \end{eqnarray} $$
台形近似による数値積分を行うことでロボットの現在位置と角度を推定できます。
すべりを抑えるためにパラメータを調整します。
#contact.surface.mu = 99
contact.surface.mu = 999
#contact.surface.bounce = 1.0
contact.surface.bounce = 0.0
ODE の drawstuff のステップ毎に数値積分します。
def __StepCallback(pause):
# ロボットの速度制御
robot.Control()
# 現在座標を表示
# x, y = robot.GetPos()
# print('{}\t{}'.format(x, y))
robot.UseJointAngleRates(tDelta=0.01)
# ...以下略
インスタンス変数としてステップ毎の積分結果を保持します。
class Robot(object):
def __init__(self, world, space, lx=0.4, ly=0.4, lz=0.2, rLR=0.11, rFB=0.05, w=0.02, mBase=9.4, mLR=0.2, mFB=0.1, px=0.0, py=0.0, pz=0.2):
self.__velL = 0.0
self.__velR = 0.0
self.__base = self.__AddBox(world, space, lx=lx, ly=ly, lz=lz, m=mBase, px=px, py=py, pz=pz)
self.__wheelL = self.__AddCylinder(world, space, r=rLR, l=w, m=mLR, px=-(lx + w) / 2.0, py=0.0, pz=(pz - lz / 2.0))
self.__wheelR = self.__AddCylinder(world, space, r=rLR, l=w, m=mLR, px=(lx + w) / 2.0, py=0.0, pz=(pz - lz / 2.0))
self.__wheelF = self.__AddCylinder(world, space, r=rFB, l=w, m=mFB, px=0.0, py=(ly / 2.0 - rFB), pz=(pz - lz / 2.0 - rFB))
self.__wheelB = self.__AddCylinder(world, space, r=rFB, l=w, m=mFB, px=0.0, py=-(ly / 2.0 - rFB), pz=(pz - lz / 2.0 - rFB))
self.__jointL = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelL), odepy.dGeomGetPosition(self.__wheelL))
self.__jointR = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelR), odepy.dGeomGetPosition(self.__wheelR))
self.__jointF = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelF), odepy.dGeomGetPosition(self.__wheelF))
self.__jointB = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelB), odepy.dGeomGetPosition(self.__wheelB))
# ロボットの重心から車輪までの距離、車輪の半径
self.__d = (lx + w) / 2.0
self.__rLR = rLR
# 左車輪と右車輪の回転速度
self.__omegaL = 0.0
self.__omegaR = 0.0
# 車体の進行方向の回転角度、現在位置
self.__phi = 0.0
self.__x = px
self.__y = py
def UseJointAngleRates(self, tDelta):
# 現時点における車輪の角速度
omegaL = odepy.dJointGetHingeAngleRate(self.__jointL)
omegaR = odepy.dJointGetHingeAngleRate(self.__jointR)
# 台形近似による数値積分
def fnPhi(omegaL, omegaR, r, d):
return r * (omegaL - omegaR) / (2.0 * d)
def fnX(omegaL, omegaR, phi, r):
return r * (omegaL + omegaR) * np.sin(phi) / 2.0
def fnY(omegaL, omegaR, phi, r):
return r * (omegaL + omegaR) * np.cos(phi) / 2.0
phi = self.__phi + (fnPhi(self.__omegaL, self.__omegaR, self.__rLR, self.__d) +
fnPhi(omegaL, omegaR, self.__rLR, self.__d)) * tDelta / 2.0
xx = self.__x + (fnX(self.__omegaL, self.__omegaR, self.__phi, self.__rLR) +
fnX(omegaL, omegaR, phi, self.__rLR)) * tDelta / 2.0
yy = self.__y + (fnY(self.__omegaL, self.__omegaR, self.__phi, self.__rLR) +
fnY(omegaL, omegaR, phi, self.__rLR)) * tDelta / 2.0
# 次のステップで利用するために結果を保存
self.__omegaL = omegaL
self.__omegaR = omegaR
self.__phi = phi
self.__x = xx
self.__y = yy
# 推定結果の確認
x, y = self.GetPos()
print(phi * 180.0 / np.pi)
# print('{}\t{}'.format(xx, x))
# print('{}\t{}'.format(yy, y))
関連ページ
概要 Open Dynamics Engine (ODE) を用いて、全方向移動ロボットの位置制御を PID 制御で行う場合のサンプルを記載します。こちらのページに記載の差分駆動型ロボットと比較して、全方向移動ロボットは任意の方向に移動できるため位置制御が容易です。 モータの角速度を操作することでロボットの位置を制御
