麦克纳姆轮的运动学和运动控制

麦克纳姆轮的运动学

各轮的转动和移动机器人整体运动的关系

L是车身半长，w是车中线到轮子中线的距离，r是轮子和轮毂的角度；

麦轮原理
u是四个轮子的角速度，r是轮子半径；

开环运动控制

各轮子的转速->车身的运动->按理想位姿到达目标位置
前半段上面已经解决，现在解决后半段。

关于控制律：
通过v和w去控制距离和两个角度这个思想特别重要。

仿真

#!/usr/bin/env python
# -*- coding: UTF-8 -*-

""" main script """

import math
import vrepInterface

class Agent(object):
    def __init__(self):
        import sys
        if sys.version_info[0] < 3:
            sys.exit("Sorry, Python 3 required")
        self.r = 0.05


    def run(self):
        vrepInterface.connect()
        vrepInterface.setup_devices()

        forwBackVel = 0
        leftRightVel = 0
        rotVel = 0

        while True:
            pos = vrepInterface.get_goal_pose()
            print(pos)

            forwBackVel =  0.06*pos[2]
            leftRightVel = 0.06*pos[1]
            angle = math.atan(pos[1]/pos[2])
            rotVel = 0.1*angle

            v_fl = (-forwBackVel-leftRightVel-0.38655*rotVel)/self.r
            v_rl = (-forwBackVel+leftRightVel-0.38655*rotVel)/self.r
            v_rr = (-forwBackVel-leftRightVel+0.38655*rotVel)/self.r
            v_fr = (-forwBackVel+leftRightVel+0.38655*rotVel)/self.r
            vrepInterface.move_wheels(v_fl, v_rl, v_rr, v_fr)

            continue




if __name__ == '__main__':
    agent = Agent()
    agent.run()

仿真gif