|
文章目录
0、问题描述：1、问题分析:2、解决方案：2.1Ackermann差速控制输出方法2.1.1理解阿克曼模型2.1.2适用阿克曼模型的teb_local_planner
2.2改正cmd_vel的输出频率，得到一个稳定的速度控制输出2.2.1输出不稳定问题2.2.2原因分析2.2.3解决思路(a)2.2.4解决思路(b)
3、程序代码
0、问题描述：
根据https://wenku.baidu.com/view/ae8058e880c758f5f61fb7360b4c2e3f57272506.html 或者其他资料，推导Ackermann差速控制输出方法，用python或c++实现。改进控制器，可选： ① 使用L1或者Pure_pursuit针对性修改。 ② 修改servo_commands.py （提示1：teb的输出为间歇脉冲式，要让小车稳定前进，控制量必须连续。提示2：servo_commands.py中AckermannMuxOutput会持续发送0信号，导致小车走走停停。） ③ 其他控制方法。
1、问题分析:
其实题目已经很清楚了，不需要过多解释，这里引用一下学长的解释： 第一个任务，修改ackermann模型输出的目的是让小车转弯更丝滑，直接在servo_commands.py里修改就可以了，有同学单独写了脚本（也没问题但是最后还是要合并到servo里哦） 第二个任务跟第一个不重合，第二个是优化cmd_vel的输出，默认10hz的输出频率，频率比较低gazebo仿真出来车会一耸一耸的，提高输出频率小车运行会更稳一些（可以考虑用rospy.Timer）；或者也可以另外加一个节点，用L1或者pure_pursuit控制小车循迹。（即 local_planner的local_plan或cmd_vel → cmd_vel优化输出/L1/Pure_pursuit → 阿克曼差速输出 → 关节pid控制器）
2、解决方案：
2.1Ackermann差速控制输出方法
2.1.1理解阿克曼模型
对于阿克曼模型的定义，网上有很多解释，这里给出两篇参考： 这是ROSwiki上的解释 这是比较清晰的解释 简单总结：对于没有万向轮的小车，在转弯时，要实现较为丝滑的转弯，需要控制四个轮子的转速和角度，尤其是前两个轮子
2.1.2适用阿克曼模型的teb_local_planner
了解了阿克曼模型，接下来就是如何解决这个问题，即将本地规划（localplanner）得到的cmd_vel转换为四个轮子的控制参数。 两个前轮转向角度和四个轮子速度计算：由于Twist消息只会给出线速度和角速度，而非线速度和转向角度，所以这种速度公式还需要转换一下,转向角度可以由轴距和转向半径的商的反正切得到。这样，核心的六个参数（四个轮子线速度和前两个轮子的角度）便得出来了。 
这是另外一篇较为详细的公式计算和编程：同时在B站有对应的视频教程 注意事项：
（1）、cmd_vel转递的是线速度和角速度，而我们需要变换成四个轮子的线速度和前轮的角度，注意，变换之后是角度而非角速度。直接通过三角函数变换即可，不过也可以通过修改参数实现： （2）、可以通过查找urdf或者xacro文件查看轴距和轮距，也可以通过在gazebo中查看四个轮子中前后和左右坐标差来计算。
2.2改正cmd_vel的输出频率，得到一个稳定的速度控制输出
2.2.1输出不稳定问题
在利用cmd_vel控制小车的输出时，通过rqt_plot可以看到小车的速度输出是类似一个个脉冲的输出，这就使得速度控制非常不稳定。 
2.2.2原因分析
究其原因，是因为在控制小车速度的节点不止一个，还有一个键盘控制的节点也会发送速度控制的参数。在小车导航时，由于没有键盘按下，该节点就会持续发送速度为零的控制命令,这样，小车行驶就会很不稳定。 
2.2.3解决思路(a)
提高发送cmd_vel的频率，从而使得cmd_vel发送更为频繁，这样，小车大多数时候接受的就是我们想要它接受的cmd_vel，而非键盘发送的消息。直接修改参数即可： 
修改后效果如下： 
2.2.4解决思路(b)
直接取消小车对键盘控制节点的订阅，这种方法简单粗暴，但效果喜人，用了的都说好，效果如下： 
3、程序代码
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import math
from std_msgs.msg import Bool
from std_msgs.msg import Float32
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
from ackermann_msgs.msg import AckermannDriveStamped
#flag_move = 0
#接受键盘消息控制小车的速度和角度
def set_throttle_steer(data):
#global flag_move
pub_vel_left_rear_wheel = rospy.Publisher('/racecar/left_rear_wheel_velocity_controller/command', Float64, queue_size=1)
pub_vel_right_rear_wheel = rospy.Publisher('/racecar/right_rear_wheel_velocity_controller/command', Float64, queue_size=1)
pub_vel_left_front_wheel = rospy.Publisher('/racecar/left_front_wheel_velocity_controller/command', Float64, queue_size=1)
pub_vel_right_front_wheel = rospy.Publisher('/racecar/right_front_wheel_velocity_controller/command', Float64, queue_size=1)
pub_pos_left_steering_hinge = rospy.Publisher('/racecar/left_steering_hinge_position_controller/command', Float64, queue_size=1)
pub_pos_right_steering_hinge = rospy.Publisher('/racecar/right_steering_hinge_position_controller/command', Float64, queue_size=1)
# Velocity is in terms of radians per second.
# Want to go 1 m/s with a wheel of radius 0.05m. This translates to 19.97 radians per second, roughly 20.
# However, at a multiplication factor of 20 speed is half of what it should be, so doubled to 40.
throttle = data.drive.speed * 40.0
steer = data.drive.steering_angle
pub_vel_left_rear_wheel.publish(throttle)
pub_vel_right_rear_wheel.publish(throttle)
pub_vel_left_front_wheel.publish(throttle)
pub_vel_right_front_wheel.publish(throttle)
pub_pos_left_steering_hinge.publish(steer)
pub_pos_right_steering_hinge.publish(steer)
#设置最大的角度
def limsteer(data,maxdata):
if data>0 and data > maxdata:
data = maxdata
elif data maxdata:
data = maxdata
return data
#转换cmd_vel的Twist消息为阿克曼模型的四个轮子的速度和前轮角度
def set_speed(data):
#global flag_move
pub_vel_left_rear_wheel = rospy.Publisher('/racecar/left_rear_wheel_velocity_controller/command', Float64, queue_size=1)
pub_vel_right_rear_wheel = rospy.Publisher('/racecar/right_rear_wheel_velocity_controller/command', Float64, queue_size=1)
pub_vel_left_front_wheel = rospy.Publisher('/racecar/left_front_wheel_velocity_controller/command', Float64, queue_size=1)
pub_vel_right_front_wheel = rospy.Publisher('/racecar/right_front_wheel_velocity_controller/command', Float64, queue_size=1)
pub_pos_left_steering_hinge = rospy.Publisher('/racecar/left_steering_hinge_position_controller/command', Float64, queue_size=1)
pub_pos_right_steering_hinge = rospy.Publisher('/racecar/right_steering_hinge_position_controller/command', Float64, queue_size=1)
# Velocity is in terms of radians per second.
# Want to go 1 m/s with a wheel of radius 0.05m. This translates to 19.97 radians per second, roughly 20.
# However, at a multiplication factor of 20 speed is half of what it should be, so doubled to 40.
x = data.linear.x
z = data.angular.z
L = 0.335 #轴距
T = 0.305 #两侧轮子之间的距离
if z!=0 and x!=0:
r=math.fabs(x/z) #转弯半径（车子中心到转弯的圆心）
rL_rear = r-(math.copysign(1,z)*(T/2.0)) #r为小车中心的转弯半径，所以T需要除以2在叠加上去
rR_rear = r+(math.copysign(1,z)*(T/2.0))
rL_front = math.sqrt(math.pow(rL_rear,2)+math.pow(L,2))
rR_front = math.sqrt(math.pow(rR_rear,2)+math.pow(L,2))
vL_rear = x*rL_rear/r
vR_rear = x*rR_rear/r
vL_front = x*rL_front/r
vR_front = x*rR_front/r
anL_front = math.atan2(L,rL_front)*math.copysign(1,z)
anR_front = math.atan2(L,rR_front)*math.copysign(1,z)
else:
vL_rear = x
vR_rear = x
vL_front =x
vR_front =x
anL_front = z
anR_front = z
anL_front = limsteer(anL_front,0.7) #最大转弯角度的弧度为0.7
anR_front = limsteer(anR_front,0.7)
pub_vel_left_rear_wheel.publish(vL_rear*100)
pub_vel_right_rear_wheel.publish(vR_rear*100)
pub_vel_left_front_wheel.publish(vL_front*100)
pub_vel_right_front_wheel.publish(vR_front*100)
pub_pos_left_steering_hinge.publish(anL_front)
pub_pos_right_steering_hinge.publish(anR_front)
def servo_commands():
rospy.init_node('servo_commands', anonymous=True)
#rospy.Subscriber("/racecar/ackermann_cmd_mux/output", AckermannDriveStamped, set_throttle_steer) #取消了键盘控制节点发布消息的订阅
rospy.Subscriber("/cmd_vel", Twist, set_speed)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
try:
servo_commands()
except rospy.ROSInterruptException:
pass
|