上位机程序：
主要介绍ros_arduino_python包
ros_arduino_python #ROS相关的Python包，用于上位机，树莓派等开发板或电脑等。
├── CMakeLists.txt
├── config #配置目录
│ └── arduino_params.yaml #定义相关参数，端口，rate，PID，sensors等默认参数。由arduino.launch调用
├── launch
│ └── arduino.launch #启动文件
├── nodes
│ └── arduino_node.py #python文件,实际处理节点，由arduino.launch调用，即可单独调用。
├── package.xml
├── setup.py
└── src #Python类包目录
└── ros_arduino_python
├── arduino_driver.py #Arduino驱动类
├── arduino_sensors.py #Arduino传感器类
├── base_controller.py #基本控制类，订阅cmd_vel话题，发布odom话题
└── __init__.py #类包默认空文件 -
arduino_params.yaml ：
- 这里建议换个名字，比如我是 my_arduino_params.yaml
//如果要直接复制使用，请删除注释
# For a direct USB cable connection, the port name is typically
# /dev/ttyACM# where is # is a number such as 0, 1, 2, etc
# For a wireless connection like XBee, the port is typically
# /dev/ttyUSB# where # is a number such as 0, 1, 2, etc.
port: /dev/ttyACM0 //arduino端口
baud: 57600 //波特率
timeout: 0.1 //超时时间
rate: 50
#sensorstate_rate: 10
use_base_controller: True //是否使用base_controller
base_controller_rate: 10 //base_controller控制频率
# For a robot that uses base_footprint, change base_frame to base_footprint
//base_link:机器人本体坐标系，与机器人中心重合，当然有些机器人(PR 2)是base_footprint,其实是一个意思。
base_frame: base_link
# === Robot drivetrain parameters
wheel_diameter: 0.0973 //车轮直径
wheel_track: 0.292 //两轮间距
encoder_resolution: 1322 # from Pololu for 30:1 motors
//编码器码盘一圈的脉冲数，可以用arduino的串口输入Ｅ获得 这里我的电机是有减速的
//但是我图方便，没有计算减速比，直接记录车轮转一圈的脉冲数
gear_reduction: 1 //减速比
motors_reversed: True //电机是否允许反向
# === PID parameters//PID参数
lKp: 25
lKd: 12
lKi: 0
lKo: 55
rKp: 25
rKd: 12
rKi: 5
rKo: 55
accel_limit: 0.5
# === Sensor definitions. Examples only - edit for your robot.
# Sensor type can be one of the follow (case sensitive!):
# * Ping
# * GP2D12
# * Analog
# * Digital
# * PololuMotorCurrent
# * PhidgetsVoltage
# * PhidgetsCurrent (20 Amp, DC)
sensors: {
#motor_current_left: {pin: 0, type: PololuMotorCurrent, rate: 5},
#motor_current_right: {pin: 1, type: PololuMotorCurrent, rate: 5},
#ir_front_center: {pin: 2, type: GP2D12, rate: 10},
#sonar_front_center: {pin: 5, type: Ping, rate: 10},
#arduino_led: {pin: 13, type: Digital, rate: 5, direction: output}
}-
arduino.launch
在这里把配置加载
<launch>
<node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
<rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
</node>
</launch>-
arduino_node.py 实际处理节点：
将这行更改成这样：发布的topic名字改为 cmd_vel
# A cmd_vel publisher so we can stop the robot when shutting down
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)将base_frame改为base_link
self.base_frame = rospy.get_param("~base_frame", 'base_link')只要注意和前后对应即可
-
arduino_driver.py Arduino驱动类：
在Arduino类中添加：
def get_pidin(self):
values = self.execute_array('i')
if len(values) != 2:
print "get_pidin count was not 2"
raise SerialException
return None
else:
return values
def get_pidout(self):
values = self.execute_array('f')
if len(values) != 2:
print "get_pidout count was not 2"
raise SerialException
return None
else:
return values将update_pid更改为下面
def update_pid(self, lKp, lKd, lKi, lKo, rKp, rKd, rKi, rKo):
''' Set the PID parameters on the Arduino
'''
print "Updating PID parameters"
cmd = 'u ' + str(lKp) + ':' + str(lKd) + ':' + str(lKi) + ':' + str(lKo)+':'+ str(rKp) + ':' + str(rKd) + ':' + str(rKi) + ':' + str(rKo)
self.execute_ack(cmd)-
base_controller.py 基本控制类
它订阅cmd_vel话题，发布odom话题
由于左右两轮使用不同的pid参数，所=所以需要做出如下改动：
在from tf.broadcaster import TransformBroadcaster下面增加
from std_msgs.msg import Int32
将pid_params = dict()后的字典进行更改
pid_params = dict()
pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
pid_params['lkp'] = rospy.get_param("~lkp", 20)
pid_params['lkd'] = rospy.get_param("~lkd", 12)
pid_params['lki'] = rospy.get_param("~lki", 0)
pid_params['lko'] = rospy.get_param("~lko", 50)
pid_params['rkp'] = rospy.get_param("~rkp", 20)
pid_params['rkd'] = rospy.get_param("~rkd", 12)
pid_params['rki'] = rospy.get_param("~rki", 0)
pid_params['rko'] = rospy.get_param("~rko", 50)将setup_pid进行如下修改：
def setup_pid(self, pid_params):
# Check to see if any PID parameters are missing
missing_params = False
for param in pid_params:
if pid_params[param] == "":
print("*** PID Parameter " + param + " is missing. ***")
missing_params = True
if missing_params:
os._exit(1)
self.wheel_diameter = pid_params['wheel_diameter']
self.wheel_track = pid_params['wheel_track']
self.encoder_resolution = pid_params['encoder_resolution']
self.gear_reduction = pid_params['gear_reduction']
self.lkp = pid_params['lkp']
self.lkd = pid_params['lkd']
self.lki = pid_params['lki']
self.lko = pid_params['lko']
self.rkp = pid_params['rkp']
self.rkd = pid_params['rkd']
self.rki = pid_params['rki']
self.rko = pid_params['rko']
self.arduino.update_pid(self.lkp, self.lkd, self.lki, self.lko,self.rkp, self.rkd, self.rki, self.rko)
self.odomPub = rospy.Publisher(‘odom’, Odometry) 上面添加如下内容：
self.lEncoderPub = rospy.Publisher('Lencoder', Int32)
self.rEncoderPub = rospy.Publisher('Rencoder', Int32)
self.lPidoutPub = rospy.Publisher('Lpidout', Int32)
self.rPidoutPub = rospy.Publisher('Rpidout', Int32)
self.lVelPub = rospy.Publisher('Lvel', Int32)
self.rVelPub = rospy.Publisher('Rvel', Int32)
在poll(self)函数的，if now > self.t_next: 下添加
try:
left_pidin, right_pidin = self.arduino.get_pidin()
except:
rospy.logerr("getpidout exception count: ")
return
self.lEncoderPub.publish(left_pidin)
self.rEncoderPub.publish(right_pidin)
try:
left_pidout, right_pidout = self.arduino.get_pidout()
except:
rospy.logerr("getpidout exception count: ")
return
self.lPidoutPub.publish(left_pidout)
self.rPidoutPub.publish(right_pidout)
在poll(self)函数的，if not self.stopped: 下添加：
self.lVelPub.publish(self.v_left)
self.rVelPub.publish(self.v_right)
运行
将小车的电源等接好
每打开一个新终端就要刷新一下环境：（进入到工作目录运行）
source devel/setup.bash 打开新终端，进入到ros_arduino_bridge所在工作空间，刷新工作环境后运行如下命令：
roslaunch ros_arduino_python arduino.launch再打开一个终端，运行键盘控制程序，这个程序可以使用turtlebot包中的robot_keyboard_teleop.py。将其发送的话题重命名即可
这时如果一切正常，小车就可以正常移动，如果转动方向什么的有问题，根据个人经验调整arduino电机驱动代码
如果一切正常，进行下一步
再打开一个新终运行如下：
rqt_plot /Lencoder /Lpidout /Lvel 可以看到实时的速度曲线，方便进行pid参数的调整。
如果小车速度不稳定，通过校准PID来改进
问题：
- 如果发现小车速度忽快忽慢，或者走不了直线，那是因为PID参数给的不合理造成的。
- PID的目的是通过改变电机PWM值，使电机实际的转速基本等于期望的转速。
- 如果参数不合理，就会出现实际的转速和期望的转速相差很远。
- 也就是说，我们没办法精准控制小车。
校准方法：
- 把电机PWM值、期望的转速和实际的转速这三者的值用图表实时地描绘出来。
- 根据PWM值和实际的转速的运动轨迹，不停地修改PID的参数，让期望的转速和实际的转速能在很短时间内的达到一致。
- 调节顺序，先调P,再调I，最后调D，通常只需要P和I两个参数就可以了。