モーダルを閉じる工作HardwareHub ロゴ画像

工作HardwareHubは、ロボット工作や電子工作に関する情報やモノが行き交うコミュニティサイトです。さらに詳しく

利用規約プライバシーポリシー に同意したうえでログインしてください。

工作HardwareHub ロゴ画像 (Laptop端末利用時)
工作HardwareHub ロゴ画像 (Mobile端末利用時)
目次目次を開く/閉じる

Open Dynamics Engine によるロボットの自己位置の推定 (Python)

モーダルを閉じる

ステッカーを選択してください

モーダルを閉じる

お支払い内容をご確認ください

購入商品
」ステッカーの表示権
メッセージ
料金
(税込)
決済方法
GooglePayマーク
決済プラットフォーム
確認事項

利用規約をご確認のうえお支払いください

※カード情報はGoogleアカウント内に保存されます。本サイトやStripeには保存されません

※記事の執筆者は購入者のユーザー名を知ることができます

※購入後のキャンセルはできません

作成日作成日
2020/05/10
最終更新最終更新
2024/11/30
記事区分記事区分
一般公開

目次

    アカウント プロフィール画像 (サイドバー)

    Software Engineer @ Tokyo

    0
    ステッカーを贈るとは?

    ロボットアプリケーションを開発する際に、ロボットの自己位置を推定する必要がある場合を考えます。ここでは 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) とよびます。マイクロマウス等で利用される加速度センサ、ジャイロ、ロータリーエンコーダは内界センサです。車輪と地面の間のすべり等が存在するため誤差が蓄積されていきます。定期的な補正が必要です。

    ロボット重心の座標を xxyy、進行方向の角度を ϕ\phi とすると、車輪の回転速度 ω\omega を用いて以下の式が成り立ちます。車輪が地面と接触している箇所における速度は rωr \omega です。dd は車体重心から車輪までの距離、rr は車輪の半径です。

    dϕ(t)dt=rωl(t)rωr(t)2ddx(t)dt=rωl(t)sinϕ(t)+rωr(t)sinϕ(t)2dy(t)dt=rωl(t)cosϕ(t)+rωr(t)cosϕ(t)2\begin{aligned} \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{aligned}

    台形近似による数値積分を行うことでロボットの現在位置と角度を推定できます。

    すべりを抑えるためにパラメータを調整します。

    #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))
    
    0
    詳細設定を開く/閉じる
    アカウント プロフィール画像 (本文下)

    Software Engineer @ Tokyo

    記事の執筆者にステッカーを贈る

    有益な情報に対するお礼として、またはコメント欄における質問への返答に対するお礼として、 記事の読者は、執筆者に有料のステッカーを贈ることができます。

    さらに詳しく →
    ステッカーを贈る コンセプト画像

    Feedbacks

    Feedbacks コンセプト画像

      ログインするとコメントを投稿できます。

      関連記事