单舵轮&阿克曼运动模型速度解算及里程计计算（附代码）

亦寻风

于 2025-01-18 14:35:49 发布

阅读量760

点赞数 6

文章标签：机器人

本文链接：https://blog.csdn.net/weixin_39506839/article/details/143511083

版权

一、中心安装单舵轮运动模型

$L$ ：车辆轴距； $R$ ：车辆转弯半径； $R_{f}$ ：前轮转弯半径； $O$ ：旋转中心；

$\alpha$ ：前轮转角； $V_{f}$ ：前轮速度；

速度解算

单舵轮速度解算功能为将控制中心的线速度及角速度转化为舵轮的角度和速度，单舵轮模型的控制中心是两个从动轮的中心，这里我们将其的线速度用 $V$ ，角速度用 $\omega$ 表示。

由圆周公式可知

$R=\frac{V}{\omega }$

由三角公式可知

$\alpha =\arctan\left ( \frac{L}{R}\right )$

$R_{f}=\frac{L}{\sin \left ( \alpha \right )}$

又由于车体为刚性连接，全车所有位置角速度 $\omega$ 相同，再联立圆周公式可知

$V_{f}=R_{f}*\omega$

  double wheelbase = 1.0; // 轴距

  // vel_x： 线速度，vel_th： 角速度
  // front_wheel_speed：前轮转角， front_wheel_speed：前轮速度
  void computeWheelVelocities(double vel_x, double vel_th, double &front_wheel_steering_angle, double &front_wheel_speed) 
  {
    if(fabs(vel_th) < 0.00001) // 防止除0
    {
      front_wheel_speed = vel_x;
      front_wheel_steering_angle = vel_th;
    }
    else
    {
      double rotation_radius = vel_x / vel_th; // 计算车体旋转半径

      front_wheel_steering_angle = atan2(wheelbase, rotation_radius); // 计算前轮转角

      double front_wheel_rotation_radius = wheelbase / sin(front_wheel_steering_angle); // 计算前轮旋转半径

      front_wheel_speed = front_wheel_rotation_radius * vel_th; // 计算前轮速度
    }
  }

里程计计算

里程计计算即为已知前轮转角 $\alpha$ 和前轮速度速 $V_{f}$ 求车辆控制中心的线速度 $V$ ，角速度 $\omega$ 。

首先可以通过前轮转角 $\alpha$ 求出车辆的旋转半径 $R$ 和前轮的旋转半径 $R_{f}$

$R=\frac{L}{\tan \alpha }$ $R_{f}=\frac{L}{\sin \alpha }$

又由于车身所有位置的角速度相同，故通过圆周公式求出车辆前轮位置的角速度，就可得出车辆的角速度。

$\omega =\frac{V_{f}}{R_{f}}=\frac{V_{f}*\sin \alpha }{L}$

最终再通过圆周公式得出车辆的速度

$V=R*\omega=V_{f}*\cos \alpha$

double wheelbase = 1.0; // 轴距
// front_wheel_steering_angle: 前轮转角，front_wheel_speed：前轮转速(m/s)
// time_interval: 两帧数据的时间间隔，odom：传出里程计数据
void UpdateOdometerInformation(double front_wheel_steering_angle, double front_wheel_speed, double time_interval, nav_msgs::Odometry &odom)
{
    odom.header.stamp = ros::Time::now();
    odom.header.frame_id = "odom";
    odom.child_frame_id = "base_link";
 
    odom.twist.twist.linear.x = front_wheel_speed* cos(front_wheel_steering_angle);
    odom.twist.twist.angular.z = front_wheel_speed* sin(front_wheel_steering_angle) / wheelbase;

    double vehicle_orientation = tf::getYaw(odom.pose.pose.orientation);
 
    double xDistance = odom.twist.twist.linear.x * time_interval * cos(vehicle_orientation); // X方向上移动的距离
    double yDistance = odom.twist.twist.linear.x * time_interval * sin(vehicle_orientation); // Y方向上移动的距离
 
    odom.pose.pose.position.x += xDistance;
    odom.pose.pose.position.y += yDistance;
 
    vehicle_orientation = angles::normalize_angle(vehicle_orientation + odom.twist.twist.angular.z * time_interval);
    odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(vehicle_orientation);
}

二、非中心安装单舵轮运动模型

$L$ ：车辆轴距； $R$ ：车辆转弯半径； $R_{c}$ ：前轮中心转弯半径； $R_{f}$ ：前轮转弯半径； $O$ ：旋转中心； $L_{d}$ ：前轮位置偏离量； $\alpha$ ：前轮中心转角； $V_{c}$ ：前轮中心位置速度；

$\alpha _{f}$ ：前轮转角； $V_{f}$ ：前轮速度；

速度解算

单舵轮除了中心安装的情况外还有非中心安装，其速度解算过程基本和中心安装的单舵轮计算方式相同，唯一区别是需要增加一个位置的修正量。

由上可知中心位置安装的舵轮计算结果为

$\alpha =\arctan\left ( \frac{L}{R_{c}}\right )$

$V_{c}=R_{c}*\omega$

同理可知前轮转角为

$\alpha _{f}=\arctan \left ( \frac{L}{R-L_{d}} \right )$

其中 $L_{d}$ 在车辆左侧为正，右侧为负。

前轮转弯半径为

$R_{f}=\frac{L}{\sin \left ( \alpha _{f}\right )}$

前轮速度为

$V_{f}=R_{f}*\omega$

double wheelbase = 1.0; // 轴距
double steering_wheel_position_offset = -0.15; // 舵轮位置偏移量，左正右负

// vel_x： 线速度，vel_th： 角速度
// front_wheel_speed：前轮转角， front_wheel_speed：前轮速度
void computeWheelVelocities(double vel_x, double vel_th, double &front_wheel_steering_angle, double &front_wheel_speed) 
{
    if(fabs(vel_th) < 0.00001) // 防止除0
    {
        front_wheel_speed = vel_x;
        front_wheel_steering_angle = vel_th;
    }
    else
    {
        double rotation_radius = vel_x / vel_th; // 计算车体旋转半径

        front_wheel_steering_angle = atan2(wheelbase, (rotation_radius - steering_wheel_position_offset)); // 计算前轮转角

        double front_wheel_rotation_radius = wheelbase / sin(front_wheel_steering_angle); // 计算前轮旋转半径

        front_wheel_speed = front_wheel_rotation_radius * vel_th; // 计算前轮速度
    }
}

里程计计算

其中角速度计算过程和中心安装车辆相同

$R_{f}=\frac{L}{\sin {\alpha _{f}} }$

$\omega =\frac{V_{f}}{R_{f}}=\frac{V_{f}*\sin{\alpha _{f}} }{L}$

线速度计算过程有所不同，由图像可知

$\tan \alpha _{f}=\frac{L}{R-{L_{d}}}$ $R=\frac{L}{\tan {\alpha _{f}}}+{L_{d}}$

车辆线速度计算为

$V=R*\omega=\left (\frac{L}{\tan {\alpha _{f}}}+{L_{d}} \right )*\frac{V_{f}*\sin{\alpha _{f}} }{L}=V_{f}\left ( \cos\alpha _{f}+\frac{\sin \alpha _{f}+L_{d}}{L} \right )$

double wheelbase = 1.0; // 轴距
double steering_wheel_position_offset = -0.15; // 舵轮位置偏移量，左正右负

// front_wheel_steering_angle: 前轮转角，front_wheel_speed：前轮转速(m/s)
// time_interval: 两帧数据的时间间隔，odom：传出里程计数据
void UpdateOdometerInformation(double front_wheel_steering_angle, double front_wheel_speed, double time_interval, nav_msgs::Odometry &odom)
{
    odom.header.stamp = ros::Time::now();
    odom.header.frame_id = "odom";
    odom.child_frame_id = "base_link";
 
    odom.twist.twist.linear.x = front_wheel_speed * (cos(front_wheel_steering_angle) + (sin(front_wheel_steering_angle) * steering_wheel_position_offset / wheelbase));
    odom.twist.twist.angular.z = front_wheel_speed * sin(front_wheel_steering_angle) / wheelbase;

    double vehicle_orientation = tf::getYaw(odom.pose.pose.orientation);
 
    double xDistance = odom.twist.twist.linear.x * time_interval * cos(vehicle_orientation); // X方向上移动的距离
    double yDistance = odom.twist.twist.linear.x * time_interval * sin(vehicle_orientation); // Y方向上移动的距离
 
    odom.pose.pose.position.x += xDistance;
    odom.pose.pose.position.y += yDistance;
 
    vehicle_orientation = angles::normalize_angle(vehicle_orientation + odom.twist.twist.angular.z * time_interval);
    odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(vehicle_orientation);
}