写在前面的话

网上找了很多资料，对于四轮转向运动学描述的很少，有部分涉及到的又将的很晦涩难懂。在论文研究大多是四轮转向动力学研究。很多文章中都会把四轮运动简化成二轮运行（自行车），但是都只讨论了前驱或者后驱的情况，把前驱或后驱中轴点的速度的速度当做整车的速度，跟我到的代码不对应。

本文是基于代码进行解析，一些前提条件本文忽略（什么刚体，侧滑之类的），把车子的中心作为运动中心，已知车速和角速度计算四轮的线速度和转向，希望大家可以看懂，主要难点是在反相运动的部分，需要着重注意。

四轮转向运动学解析

四轮转向理论图解

• 三种模式
  • 同相运动（前后轮摆动方向相同）
  • 反相运动（前后轮摆动方法相反，如下图，前轮往右摆，那么后轮就往左摆）
  • 轴心旋转

下图中属于逆向运动学，也即是根据车子行驶的速度和角速度，求解车轮的速度和方向。图中在反相运动过程中示例是右转弯，前后轮应该都符合阿克曼角，属于四驱阿克曼（双阿克曼），图中不明显。

robot_control.py 完整代码

代码链接：humble/four_ws_ros2/src/four_ws_control/scripts/robot_control.py

创建了一个 joy 节点，监听 $ve{l}_{msg}$, $mod{e}_{selection}$ 两个参数
$$\begin{array}{rl}& \begin{array}{cc}& \text{车子的速度设置：}\\ & ve{l}_{msg}=[linea{r}_x,linea{r}_y,linea{r}_z,angl{e}_x,angl{e}_y,angl{e}_z]\\ & \\ & \text{ 模式选择：}\\ & mod{e}_{selection}=[1,2,3]\\ & \end{array}\end{array}$$

关键参数

• 车轮间距（wheel_seperation）： 同一车轴上左轮和右轮之间的距离。
• 前后轮距（wheel_base）：前后车轴之间的距离。
• 车轮半径（wheel_radius）：车轮的半径。
• 车轮转向 y 偏移(wheel_steering_y_offset)： 车轮转向装置偏离车轮中心线的横向偏移。 （可以理解转向器的转轴和车轮的垂直轴有偏移）
• 转向轨迹 (steering_track)： 计算公式为【车轮间距 - 2 * 车轮转向 y 偏移】。 这表示影响车辆操控性的有效轨道宽度。

完整代码

#!/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

vel_msg = Twist()  # robot velosity
mode_selection = 0 # 1:opposite phase, 2:in-phase, 3:pivot turn 4: none

class Commander(Node):

    def __init__(self):
        super().__init__('commander')
        timer_period = 0.02
        self.wheel_seperation = 0.122
        self.wheel_base = 0.156
        self.wheel_radius = 0.026
        self.wheel_steering_y_offset = 0.03
        self.steering_track = self.wheel_seperation - 2*self.wheel_steering_y_offset

        self.pos = np.array([0,0,0,0], float)
        self.vel = np.array([0,0,0,0], float) #left_front, right_front, left_rear, right_rear

        self.pub_pos = self.create_publisher(Float64MultiArray, '/forward_position_controller/commands', 10)
        self.pub_vel = self.create_publisher(Float64MultiArray, '/forward_velocity_controller/commands', 10)
        self.timer = self.create_timer(timer_period, self.timer_callback)

    def timer_callback(self):
        global vel_msg, mode_selection

        # opposite phase
        if(mode_selection == 1):
            
            vel_steerring_offset = vel_msg.angular.z * self.wheel_steering_y_offset
            sign = np.sign(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) - vel_steerring_offset
            self.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) + vel_steerring_offset
            self.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) - vel_steerring_offset
            self.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) + vel_steerring_offset

            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-phase
        elif(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.x
            else:
                ang = 0
            
            self.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 turn
        elif(mode_selection == 3):

            self.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
            self.vel[1] = vel_msg.angular.z
            self.vel[2] = self.vel[0]
            self.vel[3] = self.vel[1]

        else:

            self.pos[:] = 0
            self.vel[:] = 0

        pos_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[:] = 0
        self.vel[:] = 0

class Joy_subscriber(Node):

    def __init__(self):
        super().__init__('joy_subscriber')
        self.subscription = self.create_subscription(
            Joy,
            'joy',
            self.listener_callback,
            10)
        self.subscription

    def listener_callback(self, data):
        global vel_msg, mode_selection

        if(data.buttons[0] == 1):   # in-phase # A button of Xbox 360 controller
            mode_selection = 2
        elif(data.buttons[4] == 1): # opposite phase # LB button of Xbox 360 controller
            mode_selection = 1
        elif(data.buttons[5] == 1): # pivot turn # RB button of Xbox 360 controller
            mode_selection = 3
        else:
            mode_selection = 4

        vel_msg.linear.x = data.axes[1]*7.5
        vel_msg.linear.y = data.axes[0]*7.5
        vel_msg.angular.z = data.axes[3]*10

if __name__ == '__main__':
    rclpy.init(args=None)
    
    commander = Commander()
    joy_subscriber = Joy_subscriber()

    executor = rclpy.executors.MultiThreadedExecutor()
    executor.add_node(commander)
    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:
        pass
    
    rclpy.shutdown()
    executor_thread.join()

公式解析（根据代码）

np.sign 是 NumPy 库中的一个函数，用于返回输入数组中每个元素的符号。它的主要作用是判断数值的正负，具体行为如下：

1. 如果元素为正数，返回 1
2. 如果元素为负数，返回 -1
3. 如果元素为零，返回 0

反相–模式1

• $v_x$ 是车子前进的速度。
• $v_z$ 是车子旋转的角速度（也就是上图中的 $w$）。
• $t$ 是车轮间距。
• $b$ 是前后轮距。
• $y_{offset}$ 是车轮和转向器的偏移。

$$\begin{array}{rl}& \\ & v_{offset}=v_z\cdot y_{offset}\\ & \\ & \\ & \text{每个车轮的线速度（除以车轮半径就是角速度）: }\\ & \begin{array}{c}{v}_{lf}=\sqrt{{\left(v_x-\frac{v_z\cdot t}{2}\right)}^2+{\left(\frac{v_z\cdot b}{2}\right)}^2}-v_{offset}\\ v_{rf}=\sqrt{{\left(v_x+\frac{v_z\cdot t}{2}\right)}^2+{\left(\frac{v_z\cdot b}{2}\right)}^2}+v_{offset}\\ v_{lr}=v_{lf}\\ v_{rr}=v_{rf}\end{array}\\ & \\ & \text{每个车轮的转向: }\\ & \begin{array}{cc}& po{s}_{lf}={\tan }^{-1}\left(\frac{v_z\cdot b}{2\cdot v_x+v_z\cdot t+0.001}\right)\\ & {\mathrm{pos}}_{rf}={\tan }^{-1}\left(\frac{v_z\cdot b}{2\cdot v_x-v_z\cdot t+0.001}\right)\end{array}\\ & \text{上式是介绍前轮的转向，后轮只要取负数即。}\end{array}$$

详细图解

根据上述的控制代码，得到车子的中心瞬时线速度$V_x$和角速度$w$，需要计算四个轮子的线速度
$$\begin{gathered} \begin{array}{r}{v}_{lf}=w\cdot \sqrt{{\left(\frac{V_x}{w}+\frac{t}{2}\right)}^2+\left({\frac{L}{2}}^2\right)}+w\cdot s\\ v_{rf}=w\cdot \sqrt{{\left(\frac{V_x}{w}-\frac{t}{2}\right)}^2+\left({\frac{L}{2}}^2\right)}-w\cdot s\end{array} \\ po{s}_{lf}=\frac{\frac{L}{2}}{\frac{V_x}{w}+\frac{t}{2}} \\ po{s}_{rf}=\frac{\frac{L}{2}}{\frac{V_x}{w}-\frac{t}{2}} \end{gathered}$$

正相–模式2

$$\begin{array}{rl}& \\ & \\ & \text{每个车轮的线速度（除以车轮半径就是角速度）: }\\ & \begin{array}{c}v=\sqrt{\left(v_x^2+v_y^2\right)}\end{array}\\ & \\ & \text{每个车轮的转向: }\\ & \begin{array}{c}pos={\tan }^{-1}\left(\frac{v_y}{v_x}\right)\end{array}\end{array}$$

轴心–模式3

$$\begin{array}{rl}& \\ & \text{每个车轮的线速度（除以车轮半径就是角速度）: }\\ & \begin{array}{c}{v}_{lf}=-v_z\\ v_{rf}=v_z\end{array}\\ & \text{后轮和前轮同相。}\\ & \\ & \text{每个车轮的转向: }\\ & \begin{array}{c}po{s}_{lf}=-{\tan }^{-1}\left(\frac{b}{t}\right)\\ po{s}_{rf}={\tan }^{-1}\left(\frac{b}{t}\right)\end{array}\\ & \text{后轮和前轮反相，后轮取负数即可。}\end{array}$$