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

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

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

工作HardwareHub ロゴ画像 (Laptop端末利用時)
工作HardwareHub ロゴ画像 (Mobile端末利用時)

PID 制御による全方向移動ロボットの位置制御 (ODE、Python)

モーダルを閉じる

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

モーダルを閉じる

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

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

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

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

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

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

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

目次

    Dockerの基礎について学びなおし中

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

    Open Dynamics Engine (ODE) を用いて、全方向移動ロボットの位置制御を PID 制御で行う場合のサンプルを記載します。差分駆動型ロボットと比較して、全方向移動ロボットは任意の方向に移動できるため位置制御が容易です。

    モータの角速度を操作することでロボットの位置を制御

    目標値 xdx_d と現在時刻における測定値 x(t)x(t) の残差 e(t)e(t) が 0 になるように、操作量 u(t)u(t) であるモータの角速度を入力していきます。KK は比例ゲイン、積分ゲイン、微分ゲインです。

    ex(t)=xdx(t)ey(t)=ydy(t)eϕ(t)=ϕdϕ(t)u(t)=Kpe(t)+Kiτ=0te(τ)dτ+Kdde(t)dt\begin{aligned} e_x(t) &= x_d - x(t) \\ e_y(t) &= y_d - y(t) \\ e_{\phi}(t) &= \phi_d - \phi(t) \\ u(t) &= K_p e(t) + K_i \int_{\tau=0}^t e(\tau) d\tau + K_d \frac{d e(t)}{d t} \end{aligned}
    • P 制御では、目標値を通り過ぎてから逆向きの操作量が発生するため目標値に綺麗に止まれません。
    • PD 制御では、操作量より大きい逆向きの外力が存在する場合に目標値に到達できません。

    クォータニオンから回転ベクトルを取得するために以下のパッケージを利用しています。

    pip install scipy
    

    sample.py

    #!/usr/bin/python
    
    # -*- coding: utf-8 -*-
    
    import odepy
    import drawstuffpy as ds
    import ctypes
    import numpy as np
    
    from scipy.spatial.transform import Rotation
    
    def Main():
        world, space, ground, contactgroup = CreateWorld()
    
        # 全方向移動ロボット
        robot = Robot(world, space)
    
        def __NearCallback(data, o1, o2):
    
            b1 = odepy.dGeomGetBody(o1)
            b2 = odepy.dGeomGetBody(o2)
    
            # ジョイントで結合されている「車輪と車体」は衝突判定しません。
            if odepy.dAreConnectedExcluding(b1, b2, odepy.dJointTypeContact):
                return
    
            # 「車輪と何か」の衝突であるか判定します。その場合は摩擦の方向を取得します。
            wheelCollision = False
            frictionDirection = odepy.dVector3()
    
            for wheel, joint in zip(robot.GetWheels(), robot.GetJoints()):
                o1IsWheel = ctypes.addressof(wheel.contents) == ctypes.addressof(o1.contents)
                o2IsWheel = ctypes.addressof(wheel.contents) == ctypes.addressof(o2.contents)
                if not (o1IsWheel or o2IsWheel):
                    continue
                wheelCollision = True
                odepy.dJointGetHingeAxis(joint, frictionDirection)
                break
    
            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]
    
                if wheelCollision:
                    contact.surface.mode = odepy.dContactFDir1 | odepy.dContactMu2
                    contact.fdir1[0] = frictionDirection[0]
                    contact.fdir1[1] = frictionDirection[1]
                    contact.fdir1[2] = frictionDirection[2]
                    contact.surface.mu = 0.0  # オムニホイールのシミュレーション (車軸方向の摩擦係数は 0 にします)
                    contact.surface.mu2 = 99  # 車輪の進行方向
    
                c = odepy.dJointCreateContact(world, contactgroup, ctypes.byref(contact))
                odepy.dJointAttach(c, b1, b2)
    
        def __StepCallback(pause):
    
            # ロボットの位置を PID 制御
            robot.Control(tDelta=0.01)
    
            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) == 'x':
                robot.SetTargetX(robot.GetTargetX() + 1.0)
            if chr(cmd) == 'X':
                robot.SetTargetX(robot.GetTargetX() - 1.0)
            if chr(cmd) == 'y':
                robot.SetTargetY(robot.GetTargetY() + 1.0)
            if chr(cmd) == 'Y':
                robot.SetTargetY(robot.GetTargetY() - 1.0)
            if chr(cmd) == 's':
                robot.SetTargetX(0.0)
                robot.SetTargetY(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, r=0.11, w=0.02, mBase=36.4, mWheel=0.2, px=0.0, py=0.0, pz=0.3):
            self.__base = self.__AddBox(world, space, lx=lx, ly=ly, lz=lz, m=mBase, px=px, py=py, pz=pz)
    
            pxWheel = (lx + w) / 2.0
            pyWheel = (ly + w) / 2.0
            pzWheel = pz - lz / 2.0
    
            q1 = odepy.dQuaternion()
            q2 = odepy.dQuaternion()
            odepy.dQFromAxisAndAngle(q1, 0, 1, 0, np.pi / 2.0)
            odepy.dQFromAxisAndAngle(q2, 1, 0, 0, np.pi / 2.0)
    
            axis1 = [1, 0, 0]
            axis2 = [0, -1, 0]
    
            self.__wheels = [
                self.__AddCylinder(world, space, r=r, l=w, m=mWheel, px=-pxWheel, py=0.0, pz=pzWheel, q=q1),
                self.__AddCylinder(world, space, r=r, l=w, m=mWheel, px=pxWheel, py=0.0, pz=pzWheel, q=q1),
                self.__AddCylinder(world, space, r=r, l=w, m=mWheel, px=0.0, py=-pyWheel, pz=pzWheel, q=q2),
                self.__AddCylinder(world, space, r=r, l=w, m=mWheel, px=0.0, py=pyWheel, pz=pzWheel, q=q2),
            ]
            self.__joints = [
                self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheels[0]), odepy.dGeomGetPosition(self.__wheels[0]), axis1),
                self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheels[1]), odepy.dGeomGetPosition(self.__wheels[1]), axis1),
                self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheels[2]), odepy.dGeomGetPosition(self.__wheels[2]), axis2),
                self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheels[3]), odepy.dGeomGetPosition(self.__wheels[3]), axis2),
            ]
    
            # 制御対象
            self.__x = px
            self.__y = py
            self.__phi = 0.0
    
            # 前のステップにおける残差
            self.__eX = 0.0
            self.__eY = 0.0
            self.__ePhi = 0.0
    
            # 残差の積分値
            self.__sX = 0.0
            self.__sY = 0.0
            self.__sPhi = 0.0
    
        def Control(self, tDelta):
    
            # ロボットの現在位置と姿勢を何らかのセンサで正確に取得できた状況を考えます。
            pos = odepy.dGeomGetPosition(self.__base)
            x = pos[0]
            y = pos[1]
    
            q = odepy.dQuaternion()
            odepy.dGeomGetQuaternion(self.__base, q)
            rot = Rotation.from_quat([q[1], q[2], q[3], q[0]])
            rotvec = rot.as_rotvec()
            phi = rotvec[2]
    
            # 残差
            eX = self.__x - x
            eY = self.__y - y
            ePhi = self.__phi - phi
    
            # 残差の積分値
            self.__sX += (self.__eX + eX) * tDelta / 2.0
            self.__sY += (self.__eY + eY) * tDelta / 2.0
            self.__sPhi += (self.__ePhi + ePhi) * tDelta / 2.0
    
            # ゲイン
            kp = 20.0
            kd = 10.0
            ki = 1.0
    
            # 操作量
            uX = kp * eX + kd * (eX - self.__eX) / tDelta + ki * self.__sX
            uY = kp * eY + kd * (eY - self.__eY) / tDelta + ki * self.__sY
            uPhi = kp * ePhi + kd * (ePhi - self.__ePhi) / tDelta + ki * self.__sPhi
    
            # モータの最大トルク
            fMax = 100
    
            # 操作量である角速度を入力します。
    
            odepy.dJointSetHingeParam(self.__joints[0], odepy.dParamVel, uY - uPhi)
            odepy.dJointSetHingeParam(self.__joints[0], odepy.dParamFMax, fMax)
    
            odepy.dJointSetHingeParam(self.__joints[1], odepy.dParamVel, uY + uPhi)
            odepy.dJointSetHingeParam(self.__joints[1], odepy.dParamFMax, fMax)
    
            odepy.dJointSetHingeParam(self.__joints[2], odepy.dParamVel, uX + uPhi)
            odepy.dJointSetHingeParam(self.__joints[2], odepy.dParamFMax, fMax)
    
            odepy.dJointSetHingeParam(self.__joints[3], odepy.dParamVel, uX - uPhi)
            odepy.dJointSetHingeParam(self.__joints[3], odepy.dParamFMax, fMax)
    
            # 次のステップで利用するため残差を保存します。
            self.__eX = eX
            self.__eY = eY
            self.__ePhi = ePhi
    
            # 残差を出力します。
            print('{}\t{}\t{}'.format(eX, eY, ePhi))
    
        def GetWheels(self):
            return self.__wheels
    
        def GetJoints(self):
            return self.__joints
    
        def GetGeoms(self):
            return [self.__base] + self.__wheels
    
        def GetTargetX(self):
            return self.__x
    
        def GetTargetY(self):
            return self.__y
    
        def SetTargetX(self, x):
            self.__x = x
    
        def SetTargetY(self, y):
            self.__y = y
    
        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, q, 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)
            odepy.dBodySetQuaternion(body, q)
            return geom
    
        def __AddJoint(self, world, body1, body2, pos, axis):
            joint = odepy.dJointCreateHinge(world, 0)
            odepy.dJointAttach(joint, body1, body2)
            odepy.dJointSetHingeAxis(joint, *axis)
            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()
    
    0
    詳細設定を開く/閉じる
    アカウント プロフィール画像 (本文下)

    Dockerの基礎について学びなおし中

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

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

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

    Feedbacks

    Feedbacks コンセプト画像

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

      関連記事