四轮转向轮式里程计设计（python）

写在前面的话

上一篇博客：键盘控制车子四轮转向

这篇文章通过订阅车轮的速度和转向计算得到整车的速度和转向，同时发布轮式里程计信息，需要和 四轮转向键盘控制改进版 ros2（python） 进行配套使用

大部分的车子的底盘控制器其实不需要自己写轮式里程计，因为ros2有很多官方的底盘控制器，比如两轮差速等常见底盘控制器，这些官方底盘控制器会自带轮式里程计。本文用的不是官方的四轮转向底盘，所以需要再写一个轮式里程计。（耗时1-2个星期）

参考教程

官方教程

1 里程计设置
2 Publishing Odometry Information over ROS

参考代码（c++）

Autonomous-Ackerman-Simulation/four_ws_navigation/four_wheel_steering_controller/src

关键代码解析

订阅车轮速度

订阅话题/forward_velocity_controller/commands，回调函数self.velocity_callback

self.wheel_v_sub = self.create_subscription(Float64MultiArray, '/forward_velocity_controller/commands', self.velocity_callback, 10)def velocity_callback(self, msg):# 处理 '/forward_velocity_controller/commands' 话题的消息# self.get_logger().info('Received data from /forward_velocity_controller/commands: %s' % list(msg.data))self.wheel_v_array = list(msg.data)

订阅车轮转向

订阅话题/forward_position_controller/commands，回调函数self.position_callback

self.wheel_w_sub = self.create_subscription(Float64MultiArray, '/forward_position_controller/commands', self.position_callback, 10)def position_callback(self, msg):# 处理 '/forward_position_controller/commands' 话题的消息# self.get_logger().info('Received data from /forward_position_controller/commands: %s' % list(msg.data))self.wheel_w_array = list(msg.data)

订阅四轮转向控制模式

订阅话题/keyboard_control_mode，回调函数self.control_mode_callback

self.mode_sub = self.create_subscription(Int32, '/keyboard_control_mode', self.control_mode_callback, 10)def control_mode_callback(self, msg):# 处理 '/control_4ws_mode 话题的消息# self.get_logger().info('Received data from /control_4ws_mode: %s' % msg.data)self.control_mode = msg.data

积累速度和转向角

self.current_time = self.get_clock().now()
dt = (self.current_time - self.last_time).nanoseconds / 1e9self.vx, self.vy, self.vth = self.forward_kinematics(self.control_mode, self.wheel_v_array, self.wheel_w_array)
self.get_logger().info(f'\n self.x:{self.x}, self.y:{self.y}, self.th:{self.th}, dt:{dt}')
self.get_logger().info(f'\n self.vx:{self.vx}, self.vy:{self.vy}, self.vth:{self.vth}')
#====================== publish odom ====================#delta_x = (self.vx * math.cos(self.th) - self.vy * math.sin(self.th))*dt
delta_y = (self.vx * math.sin(self.th) + self.vy * math.cos(self.th))*dt
delta_th = self.vth*dtself.th += delta_th
self.x += delta_x
self.y += delta_y# self.get_logger().info(f'\n self.x:{self.x}, self.y:{self.y}, self.th:{self.th}')
self.last_time = self.current_time

发布里程计

self.odom_pub = self.create_publisher(Odometry, '/odom', 50)
self.odom_broadcaster = TransformBroadcaster(self)odom_quat = Quaternion()
# magnitude = math.sqrt(odom_quat.x**2 + odom_quat.y**2 
#                       + math.sin(self.th / 2)**2 + math.cos(self.th / 2)**2)# 归一化四元数
# odom_quat.z /= magnitude
# odom_quat.w /= magnitudeodom_quat.z = math.sin(self.th / 2)
odom_quat.w = math.cos(self.th / 2)# Publish the transform
odom_trans = TransformStamped()
odom_trans.header.stamp = self.current_time.to_msg()
odom_trans.header.frame_id = 'odom'
odom_trans.child_frame_id = 'base_link'
odom_trans.transform.translation.x = self.x
odom_trans.transform.translation.y = self.y
odom_trans.transform.translation.z = 0.0
odom_trans.transform.rotation = odom_quatself.odom_broadcaster.sendTransform(odom_trans)# Publish the odometry message
odom = Odometry()
odom.header.stamp = self.current_time.to_msg()
odom.header.frame_id = 'odom'
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = odom_quat
odom.child_frame_id = 'base_link'
odom.twist.twist.linear = Vector3(x=self.vx, y=self.vy, z=0.0)
odom.twist.twist.angular = Vector3(x=0.0, y=0.0, z=self.vth)
self.odom_pub.publish(odom)

完整代码

Class Commdar(Node) 通过整车速度和转向控制车轮速度和车轮转向
Class OdometryPublisher(Node) 的 forward_kinematics 函数通过车轮速度和车轮转向计算整车速度和转向

#!/usr/bin/python3
import math
import threading
import rclpy
import numpy as np
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy
import sys, select, termios, tty
from nav_msgs.msg import Odometry
import pty
from geometry_msgs.msg import Quaternion, TransformStamped, Twist, Vector3
from nav_msgs.msg import Odometry
from tf2_ros import TransformBroadcaster
from tf2_ros import TransformStamped
import math
from std_msgs.msg import Int32class Commander(Node):'''wheel_separation:Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.wheel_base:Distance between front and rear wheels, in meters.wheel_radius:Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.'''def __init__(self):super().__init__('commander')self.wheel_seperation = 1.2self.wheel_base = 1self.wheel_radius = 0.3self.wheel_steering_y_offset = 0.03self.steering_track = self.wheel_seperation - 2*self.wheel_steering_y_offset #1.14self.pos = np.array([0,0,0,0], float)self.vel = np.array([0,0,0,0], float) #left_front, right_front, left_rear, right_rearself.pub_pos = self.create_publisher(Float64MultiArray, '/forward_position_controller/commands', 10)self.pub_vel = self.create_publisher(Float64MultiArray, '/forward_velocity_controller/commands', 10)self.sub_vel = self.create_subscription(Twist,'/keyboard_vel_msg', self.vel_callback, 10)self.sub_mode = self.create_subscription(Int32, '/keyboard_control_mode', self.mode_callback, 10)self.sub_velself.sub_modeself.vel_msg = Twist()self.vel_msg.linear.x = 0.0self.vel_msg.linear.y = 0.0self.vel_msg.angular.x = 0.0 self.vel_msg.angular.y = 0.0 self.vel_msg.angular.z = 0.0self.control_mode = Int32()self.control_mode.data = 1self.create_timer(timer_period_sec=0.02, callback=self.timer_callback)def vel_callback(self, msg):self.vel_msg = msgreturndef mode_callback(self, msg):self.control_mode = msg.datareturn def timer_callback(self):vel_msg = self.vel_msgmode_selection = self.control_mode# opposite phaseif(mode_selection == 1):vel_steerring_offset = vel_msg.angular.z * self.wheel_steering_y_offset / self.wheel_radiussign = math.copysign(1.0, vel_msg.linear.x)            self.vel[0] = sign*math.hypot(vel_msg.linear.x - vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2)/self.wheel_radius - vel_steerring_offsetself.vel[1] = sign*math.hypot(vel_msg.linear.x + vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2)/self.wheel_radius + vel_steerring_offsetself.vel[2] = sign*math.hypot(vel_msg.linear.x - vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2)/self.wheel_radius - vel_steerring_offsetself.vel[3] = sign*math.hypot(vel_msg.linear.x + vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2)/self.wheel_radius + vel_steerring_offsetif abs(2.0 * vel_msg.linear.x) > abs(vel_msg.angular.z * self.steering_track) and math.fabs(vel_msg.linear.y) < 0.001:front_left_steering = math.atan(vel_msg.angular.z * self.wheel_base / (2.0 * vel_msg.linear.x - vel_msg.angular.z * self.steering_track))front_right_steering = math.atan(vel_msg.angular.z * self.wheel_base / (2.0 * vel_msg.linear.x + vel_msg.angular.z * self.steering_track))self.pos[0] = front_left_steeringself.pos[1] = front_right_steeringelif abs(vel_msg.linear.x) > 0.001 and math.fabs(vel_msg.linear.y) < 0.001:front_left_steering = math.copysign(math.pi / 2, vel_msg.angular.z)front_right_steering = math.copysign(math.pi / 2, vel_msg.angular.z)self.pos[0] = front_left_steeringself.pos[1] = front_right_steering# self.pos[0] = math.atan(vel_msg.angular.z*self.wheel_base/(2*vel_msg.linear.x + vel_msg.angular.z*self.steering_track+0.001))# self.pos[1] = math.atan(vel_msg.angular.z*self.wheel_base/(2*vel_msg.linear.x - vel_msg.angular.z*self.steering_track+0.001))self.pos[2] = -self.pos[0]self.pos[3] = -self.pos[1]# in-phaseelif(mode_selection == 2):V = math.hypot(vel_msg.linear.x, vel_msg.linear.y)sign = np.sign(vel_msg.linear.x)if(vel_msg.linear.x != 0):ang = vel_msg.linear.y / vel_msg.linear.xelse:ang = 0self.pos[0] = math.atan(ang)self.pos[1] = math.atan(ang)self.pos[2] = self.pos[0]self.pos[3] = self.pos[1]self.vel[:] = sign*V            # pivot turnelif(mode_selection == 3):omega_const = 6  # to make robot rotates a little bit slowerself.pos[0] = -math.atan(self.wheel_base/self.steering_track)self.pos[1] = math.atan(self.wheel_base/self.steering_track)self.pos[2] = math.atan(self.wheel_base/self.steering_track)self.pos[3] = -math.atan(self.wheel_base/self.steering_track)self.vel[0] = -vel_msg.angular.z/(omega_const * self.wheel_radius)self.vel[1] = vel_msg.angular.z/(omega_const * self.wheel_radius)self.vel[2] = self.vel[0]self.vel[3] = self.vel[1]else:self.pos[:] = 0self.vel[:] = 0# fixed by xcg: change the sign of the positionself.pos = -self.pospos_array = Float64MultiArray(data=self.pos) vel_array = Float64MultiArray(data=self.vel) self.pub_pos.publish(pos_array)self.pub_vel.publish(vel_array)self.pos[:] = 0self.vel[:] = 0class OdometryPublisher(Node):def __init__(self):super().__init__('odometry_publisher')self.wheel_v_sub = self.create_subscription(Float64MultiArray, '/forward_velocity_controller/commands', self.velocity_callback, 10)self.wheel_w_sub = self.create_subscription(Float64MultiArray, '/forward_position_controller/commands', self.position_callback, 10)self.mode_sub = self.create_subscription(Int32, '/keyboard_control_mode', self.control_mode_callback, 10)self.wheel_v_sub  # 防止被 GC 回收self.wheel_w_subself.mode_subself.odom_pub = self.create_publisher(Odometry, '/odom', 50)self.odom_broadcaster = TransformBroadcaster(self)self.get_logger().info('Subscribed to /forward_velocity(position)_controller/commands topic')self.x = 0.0self.y = 0.0self.th = 0.0self.vx = 0.0self.vy = 0.0self.vth = 0.0self.wheel_base_ = 1.0 self.wheel_seperation = 1.2self.steering_track_ = 1.14self.wheel_steering_y_offset_ = 0.03self.wheel_radius_ = 0.3 #轮子半径30cmself.wheel_v_array = [0,0,0,0]self.wheel_w_array = [0,0,0,0]self.control_mode = 1self.current_time = self.get_clock().now()self.last_time = self.get_clock().now()timer_period = 0.1self.timer = self.create_timer(timer_period, self.timer_callback)def velocity_callback(self, msg):# 处理 '/forward_velocity_controller/commands' 话题的消息# self.get_logger().info('Received data from /forward_velocity_controller/commands: %s' % list(msg.data))self.wheel_v_array = list(msg.data)def position_callback(self, msg):# 处理 '/forward_position_controller/commands' 话题的消息# self.get_logger().info('Received data from /forward_position_controller/commands: %s' % list(msg.data))self.wheel_w_array = list(msg.data)def control_mode_callback(self, msg):# 处理 '/control_4ws_mode 话题的消息# self.get_logger().info('Received data from /control_4ws_mode: %s' % msg.data)self.control_mode = msg.datadef forward_kinematics(self, control_mode, wheel_velocity, wheel_angular):# transform wheel_velocity and wheel_angular to the whole carfl_speed = wheel_velocity[0]#车轮线速度，没除车轮半径fr_speed = wheel_velocity[1]rl_speed = wheel_velocity[2]rr_speed = wheel_velocity[3]fl_steering = wheel_angular[0]#轮子转角fr_steering = wheel_angular[1]rl_steering = wheel_angular[2]rr_steering = wheel_angular[3]# 检查转向角是否为 NaNif any(math.isnan(steering) for steering in [fl_steering, fr_steering, rl_steering, rr_steering]):return# 计算前轮转向位置front_steering = 0.0if abs(fl_steering) > 0.001 or abs(fr_steering) > 0.001:# self.get_logger().info(f'\n fl_steering:{fl_steering}, math.tan(fl_steering):{math.tan(fl_steering)}, fr_steering:{fr_steering}')if control_mode != 3:front_steering = math.atan(2 * math.tan(fl_steering) * math.tan(fr_steering) / (math.tan(fl_steering) + math.tan(fr_steering))#pivot模式下正负为0)else:front_steering = 0.0# 计算后轮转向位置rear_steering = 0.0if abs(rl_steering) > 0.001 or abs(rr_steering) > 0.001:if control_mode != 3:rear_steering = math.atan(2 * math.tan(rl_steering) * math.tan(rr_steering) / (math.tan(rl_steering) + math.tan(rr_steering)))else:rear_steering = 0.0front_tmp = math.cos(front_steering) * (math.tan(front_steering) - math.tan(rear_steering)) / self.wheel_base_front_left_tmp = front_tmp / math.sqrt(1 - self.steering_track_ * front_tmp * math.cos(front_steering)+ (self.steering_track_ * front_tmp / 2)**2)front_right_tmp = front_tmp / math.sqrt(1 + self.steering_track_ * front_tmp * math.cos(front_steering)+ (self.steering_track_ * front_tmp / 2)**2)fl_speed_tmp = fl_speed * (1 / (1 - self.wheel_steering_y_offset_ * front_left_tmp))fr_speed_tmp = fr_speed * (1 / (1 - self.wheel_steering_y_offset_ * front_right_tmp))front_linear_speed = self.wheel_radius_ * math.copysign(1.0, fl_speed_tmp + fr_speed_tmp) * math.sqrt((fl_speed**2 + fr_speed**2) / (2 + (self.steering_track_ * front_tmp)**2 / 2.0))rear_tmp = math.cos(rear_steering) * (math.tan(front_steering) - math.tan(rear_steering)) / self.wheel_base_rear_left_tmp = rear_tmp / math.sqrt(1 - self.steering_track_ * rear_tmp * math.cos(rear_steering)+ (self.steering_track_ * rear_tmp / 2)**2)rear_right_tmp = rear_tmp / math.sqrt(1 + self.steering_track_ * rear_tmp * math.cos(rear_steering)+ (self.steering_track_ * rear_tmp / 2)**2)rl_speed_tmp = rl_speed * (1 / (1 - self.wheel_steering_y_offset_ * rear_left_tmp))rr_speed_tmp = rr_speed * (1 / (1 - self.wheel_steering_y_offset_ * rear_right_tmp))rear_linear_speed = self.wheel_radius_ * math.copysign(1.0, rl_speed_tmp + rr_speed_tmp) * math.sqrt((rl_speed_tmp**2 + rr_speed_tmp**2) / (2 + (self.steering_track_ * rear_tmp)**2 / 2.0))if fl_speed == 0:return 0.0, 0.0, 0.0if control_mode == 1:#opposite phaseself.angular_ = (front_linear_speed * front_tmp + rear_linear_speed * rear_tmp) / 2.0self.linear_x_ = (front_linear_speed * math.cos(front_steering) + rear_linear_speed * math.cos(rear_steering)) / 2.0self.linear_y_ = (front_linear_speed * math.sin(front_steering) - self.wheel_base_ * self.angular_ / 2.0 +rear_linear_speed * math.sin(rear_steering) + self.wheel_base_ * self.angular_ / 2.0) / 2.0# print('opposite phase mode...')elif control_mode == 2:#in-phasec_sign = math.copysign(1.0, rl_speed)self.linear_x_ = c_sign * math.sqrt(fl_speed**2 / (1 + math.tan(fl_steering)**2))self.linear_y_ = math.tan(fl_steering) * self.linear_x_self.angular_ = 0.0print('in-phase mode...')elif control_mode == 3:self.linear_x_ = 0.0self.linear_y_ = 0.0R = math.sqrt((self.wheel_base_ / 2) ** 2 + (self.steering_track_ / 2) ** 2) + self.wheel_steering_y_offset_self.angular_ = - self.wheel_radius_ * fl_speed / Rprint('pivot turn mode...')else:self.linear_x_ = 0.0self.linear_y_ = 0.0self.angular_ = 0.0print('other mode...')# print('\nn调用函数了马???')self.get_logger().info(f'\n self.linear_x_:{self.linear_x_}, self.linear_y_:{self.linear_y_}, self.angular_:{self.angular_}, control_mode:{control_mode}')return self.linear_x_, self.linear_y_, self.angular_def timer_callback(self):self.current_time = self.get_clock().now()dt = (self.current_time - self.last_time).nanoseconds / 1e9self.vx, self.vy, self.vth = self.forward_kinematics(self.control_mode, self.wheel_v_array, self.wheel_w_array)self.get_logger().info(f'\n self.x:{self.x}, self.y:{self.y}, self.th:{self.th}, dt:{dt}')self.get_logger().info(f'\n self.vx:{self.vx}, self.vy:{self.vy}, self.vth:{self.vth}')#====================== publish odom ====================#delta_x = (self.vx * math.cos(self.th) - self.vy * math.sin(self.th))*dtdelta_y = (self.vx * math.sin(self.th) + self.vy * math.cos(self.th))*dtdelta_th = self.vth*dtself.th += delta_thself.x += delta_xself.y += delta_y# self.get_logger().info(f'\n self.x:{self.x}, self.y:{self.y}, self.th:{self.th}')self.last_time = self.current_timeodom_quat = Quaternion()# magnitude = math.sqrt(odom_quat.x**2 + odom_quat.y**2 #                       + math.sin(self.th / 2)**2 + math.cos(self.th / 2)**2)# 归一化四元数# odom_quat.z /= magnitude# odom_quat.w /= magnitudeodom_quat.z = math.sin(self.th / 2)odom_quat.w = math.cos(self.th / 2)# Publish the transformodom_trans = TransformStamped()odom_trans.header.stamp = self.current_time.to_msg()odom_trans.header.frame_id = 'odom'odom_trans.child_frame_id = 'base_link'odom_trans.transform.translation.x = self.xodom_trans.transform.translation.y = self.yodom_trans.transform.translation.z = 0.0odom_trans.transform.rotation = odom_quatself.odom_broadcaster.sendTransform(odom_trans)# Publish the odometry messageodom = Odometry()odom.header.stamp = self.current_time.to_msg()odom.header.frame_id = 'odom'odom.pose.pose.position.x = self.xodom.pose.pose.position.y = self.yodom.pose.pose.position.z = 0.0odom.pose.pose.orientation = odom_quatodom.child_frame_id = 'base_link'odom.twist.twist.linear = Vector3(x=self.vx, y=self.vy, z=0.0)odom.twist.twist.angular = Vector3(x=0.0, y=0.0, z=self.vth)self.odom_pub.publish(odom)class Joy_subscriber(Node):def __init__(self):super().__init__('joy_subscriber')self.subscription = self.create_subscription(Joy,'joy',self.listener_callback,10)self.subscriptiondef listener_callback(self, data):global vel_msg, mode_selectionif(data.buttons[0] == 1):   # in-phase # A button of Xbox 360 controllermode_selection = 2elif(data.buttons[4] == 1): # opposite phase # LB button of Xbox 360 controllermode_selection = 1elif(data.buttons[5] == 1): # pivot turn # RB button of Xbox 360 controllermode_selection = 3else:mode_selection = 4vel_msg.linear.x = data.axes[1]*7.5vel_msg.linear.y = data.axes[0]*7.5vel_msg.angular.z = data.axes[3]*10if __name__ == '__main__':rclpy.init(args=sys.argv)commander = Commander()#需要, 缺少无法发布odom话题# joy_subscriber = Joy_subscriber()odom_pub = OdometryPublisher()executor = rclpy.executors.MultiThreadedExecutor()executor.add_node(commander)executor.add_node(odom_pub)# executor.add_node(joy_subscriber)executor_thread = threading.Thread(target=executor.spin, daemon=True)executor_thread.start()rate = commander.create_rate(2)try:while rclpy.ok():rate.sleep()except KeyboardInterrupt:passrclpy.shutdown()executor_thread.join()

完整视频演示

四轮转向轮式里程计设计 G果-CSDN