概要

ロボットアプリケーションを開発する際に、ロボットの自己位置を推定する必要がある場合を考えます。ここでは 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))