• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

移动机器人技术(9)– 全向移动机器人Modeling and Control

人工智能 Techblog of HaoWANG 2190次浏览 0个评论

之前写的文章分析了全向小车运动原理和基本构型,今天尝试把它部署到 Ros 上。


基本原理

参考系的定义需要根据 Ros by Example chapter 7 做一些修改:

  • 定义三轮车的三个轮子分别是 A、B、C, 速度分别是a、b、c;
  • 定义半径 Radius 是中点到轮子的距离;
  • 定义 a,b 为前轮,c 为后轮。

控制程序

Ros 中的的速度消息是 Twist 指定的

$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

linear 中的 x 代表前进的速度,单位是 m/s 。angular 中的 z 表示角速度,单位 rad/s。

python 实现,由于小车控制器和电机自带PID控制,只需写一个发布速度的节点:

#!/usr/bin/env python
# -*- coding: utf-8 -*-
 
import rospy
from geometry_msgs.msg import Twist
from math import sqrt
import RPi.GPIO as GPIO
import time
import sys
 
Radius = 10 #圆盘半径,需要根据情况修改
 
PWMA = 18
AIN1 = 22
AIN2 = 27
 
PWMB = 23
BIN1 = 25
BIN2 = 24
 
PWMC = 16
CIN1 = 20
CIN2 = 21
 
GPIO.setwarnings(False) 
GPIO.setmode(GPIO.BCM)
 
GPIO.setup(AIN2,GPIO.OUT)
GPIO.setup(AIN1,GPIO.OUT)
GPIO.setup(PWMA,GPIO.OUT)
 
GPIO.setup(BIN1,GPIO.OUT)
GPIO.setup(BIN2,GPIO.OUT)
GPIO.setup(PWMB,GPIO.OUT)
 
GPIO.setup(CIN1,GPIO.OUT)
GPIO.setup(CIN2,GPIO.OUT)
GPIO.setup(PWMC,GPIO.OUT)
 
A_Motor= GPIO.PWM(PWMA,100)
A_Motor.start(0)
 
B_Motor = GPIO.PWM(PWMB,100)
B_Motor.start(0)
 
C_Motor = GPIO.PWM(PWMC,100)
C_Motor.start(0)
 
def compute(Vx,Vy,omega):
    a = -sqrt(3)/3 * Vx + 1/3 * Vy + omega * Radius
    b =  sqrt(3)/3 * Vx + 1/3 * Vy + omega * Radius
    c =                 - 2/3 * Vy + omega * Radius
       
    return a,b,c 
 
def move(va,vb,vc,t_time = 4):
    A_Motor.ChangeDutyCycle(abs(va))
    GPIO.output(AIN1,bool(va<0))
    GPIO.output(AIN2,bool(va>0))
 
    B_Motor.ChangeDutyCycle(abs(vb))
    GPIO.output(BIN1,bool(vb<0))
    GPIO.output(BIN2,bool(vb>0))
    
    C_Motor.ChangeDutyCycle(abs(vc))
    GPIO.output(CIN1,bool(vc<0))
    GPIO.output(CIN2,bool(vc>0))
 
    time.sleep(t_time)
 
def callback(msg):
    rospy.loginfo("Received a /cmd_vel message!")
    rospy.loginfo("Linear Velocity: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z))
    rospy.loginfo("Angular Velocity: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z))
 
    # 速度获取
    omega = msg.angular.z #角速度
    Vx = msg.linear.x
    Vy = msg.linear.y
 
    va,vb,vc = compute(Vx, Vy, omega)
    
    rospy.loginfo("va: %f, vb: %f, vc: %f]"%(va, vb, vc))
 
    move(va,vb,vc)
 
def listener():
    rospy.init_node('vel_listener')
    rospy.Subscriber('/cmd_vel', Twist, callback)
    rospy.spin()
 
if __name__ == "__main__":
    try:
        listener()
    except KeyboardInterrupt:
        GPIO.cleanup()


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明移动机器人技术(9)– 全向移动机器人Modeling and Control
喜欢 (0)

您必须 登录 才能发表评论!

加载中……