126 lines
4.5 KiB
C#
126 lines
4.5 KiB
C#
using System;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.Control.Allocation
|
|
{
|
|
/// <summary>
|
|
/// 在对称前后GCP方向命令与车体中心刚体速度之间执行纯几何转换。
|
|
/// </summary>
|
|
public static class GcpKinematics
|
|
{
|
|
private const double ParallelDirectionTolerance = 1e-9;
|
|
private const double StopSpeedDeadbandMetersPerSecond = 1e-6;
|
|
|
|
/// <summary>
|
|
/// 将有符号中心速度和前后GCP方向转换为真实车体坐标系中的Twist2D。
|
|
/// </summary>
|
|
public static Twist2D ToBodyTwist(
|
|
GcpMotionCommand command,
|
|
double controlPointRadiusMeters)
|
|
{
|
|
NumericGuard.EnsureFinitePositive(
|
|
controlPointRadiusMeters,
|
|
nameof(controlPointRadiusMeters));
|
|
|
|
if (Math.Abs(command.SpeedMetersPerSecond) <=
|
|
StopSpeedDeadbandMetersPerSecond)
|
|
{
|
|
return Twist2D.Zero;
|
|
}
|
|
|
|
var frontAngleRadians =
|
|
command.FrontAngleRadians;
|
|
var rearAngleRadians =
|
|
command.RearAngleRadians;
|
|
var directionDeterminant =
|
|
Math.Sin(
|
|
rearAngleRadians -
|
|
frontAngleRadians);
|
|
|
|
if (Math.Abs(directionDeterminant) <=
|
|
ParallelDirectionTolerance)
|
|
{
|
|
var averageDirectionRadians =
|
|
Math.Atan2(
|
|
Math.Sin(frontAngleRadians) +
|
|
Math.Sin(rearAngleRadians),
|
|
Math.Cos(frontAngleRadians) +
|
|
Math.Cos(rearAngleRadians));
|
|
|
|
return new Twist2D(
|
|
command.SpeedMetersPerSecond *
|
|
Math.Cos(averageDirectionRadians),
|
|
command.SpeedMetersPerSecond *
|
|
Math.Sin(averageDirectionRadians),
|
|
0.0);
|
|
}
|
|
|
|
var frontCosine =
|
|
Math.Cos(frontAngleRadians);
|
|
var frontSine =
|
|
Math.Sin(frontAngleRadians);
|
|
var rearCosine =
|
|
Math.Cos(rearAngleRadians);
|
|
var rearSine =
|
|
Math.Sin(rearAngleRadians);
|
|
|
|
// 两个GCP速度方向的法线交点就是瞬时旋转中心，坐标位于真实车体系。
|
|
var rotationCenterXMeters =
|
|
controlPointRadiusMeters *
|
|
(frontCosine * rearSine +
|
|
frontSine * rearCosine) /
|
|
directionDeterminant;
|
|
var rotationCenterYMeters =
|
|
-2.0 *
|
|
controlPointRadiusMeters *
|
|
frontCosine *
|
|
rearCosine /
|
|
directionDeterminant;
|
|
var centerRadiusMeters =
|
|
Math.Sqrt(
|
|
rotationCenterXMeters *
|
|
rotationCenterXMeters +
|
|
rotationCenterYMeters *
|
|
rotationCenterYMeters);
|
|
|
|
if (!NumericGuard.IsFinite(centerRadiusMeters) ||
|
|
centerRadiusMeters <= 0.0)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"前后GCP方向不能生成有效的车体中心旋转半径。");
|
|
}
|
|
|
|
// 用有符号速度决定绕ICR的实际转向，避免倒车时把同一组GCP轴向解释成反向运动。
|
|
var requestedDirectionSign =
|
|
Math.Sign(command.SpeedMetersPerSecond);
|
|
var positiveAngularFrontVelocityXMetersPerSecond =
|
|
rotationCenterYMeters;
|
|
var positiveAngularFrontVelocityYMetersPerSecond =
|
|
controlPointRadiusMeters -
|
|
rotationCenterXMeters;
|
|
var frontDirectionAlignment =
|
|
positiveAngularFrontVelocityXMetersPerSecond *
|
|
requestedDirectionSign *
|
|
frontCosine +
|
|
positiveAngularFrontVelocityYMetersPerSecond *
|
|
requestedDirectionSign *
|
|
frontSine;
|
|
var omegaSign =
|
|
frontDirectionAlignment >= 0.0
|
|
? 1.0
|
|
: -1.0;
|
|
var omegaRadiansPerSecond =
|
|
omegaSign *
|
|
Math.Abs(command.SpeedMetersPerSecond) /
|
|
centerRadiusMeters;
|
|
|
|
return new Twist2D(
|
|
omegaRadiansPerSecond *
|
|
rotationCenterYMeters,
|
|
-omegaRadiansPerSecond *
|
|
rotationCenterXMeters,
|
|
omegaRadiansPerSecond);
|
|
}
|
|
}
|
|
}
|