底盘四轮转向运动学解析（含代码）

写在前面的话

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

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

四轮转向运动学解析

四轮转向理论图解

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

在这里插入图片描述

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

在这里插入图片描述

robot_control.py 完整代码

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

创建了一个 joy 节点，监听 $vel_{msg}$ , $mode_{selection}$ 两个参数
$\begin{aligned} &\begin{gathered} &\text{车子的速度设置：}\\ &vel_{ msg} = [linear_x, linear_y, linear_z, angle_x, angle_y, angle_z]\\ \\ &\text{ 模式选择：}\\ &mode_{selection} = [1, 2, 3] \\ &\end{gathered} \end{aligned}$

关键参数

车轮间距（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
如果元素为零，返回 0

反相–模式1

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

$\begin{aligned}\\ &v_{offset} = v_z \cdot y_{offset} \\ \\ \\ &\text {每个车轮的线速度（除以车轮半径就是角速度）: }\\ &\begin{gathered} v_{l f}= \sqrt{\left(v_x-\frac{v_z \cdot t}{2}\right)^2+\left(\frac{v_z \cdot b}{2}\right)^2}-v_{offset} \\ v_{r f}=\sqrt{\left(v_x+\frac{v_z \cdot t}{2}\right)^2+\left(\frac{v_z \cdot b}{2}\right)^2}+v_{offset} \\ v_{l r}=v_{l f} \\ v_{r r}=v_{r f} \end{gathered}\\ \\ &\text {每个车轮的转向: }\\ &\begin{gathered} & p o s_{l f}=\tan ^{-1}\left(\frac{v_z \cdot b}{2 \cdot v_x+v_z \cdot t + 0.001}\right) \\ & \operatorname{pos}_{r f}=\tan ^{-1}\left(\frac{v_z \cdot b}{2 \cdot v_x-v_z \cdot t +0.001}\right) \end{gathered}\\ &\text {上式是介绍前轮的转向，后轮只要取负数即。}\\ \end{aligned}$

详细图解

根据上述的控制代码，得到车子的中心瞬时线速度 $V_x$ 和角速度 $w$ ，需要计算四个轮子的线速度
$\begin{aligned} 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{aligned} \\ pos_{lf} = \frac{\frac{L}{2}}{\frac{V_x}{w}+\frac{t}{2}} \\ pos_{rf} = \frac{\frac{L}{2}}{\frac{V_x}{w}-\frac{t}{2}}$

在这里插入图片描述

正相–模式2

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

轴心–模式3

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