First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

完善蟹行虚拟阿克曼与SendMotion运动坐标系，并添加轮速诊断日志

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
yuxiang.shen
2026-07-30 18:10:22 +08:00
co-authored by Cursor
parent 7e05ff098e
commit c1fe73caec
92 changed files with 1794 additions and 776 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs
+408 -245
View File
@@ -10,19 +10,19 @@ using System.Drawing;
namespace CommonUsage.Chassis
{
public class MultiWheelChassis : AbstractChassis
{
public float ThConsistentThreshold = 1.0f;
public class MultiWheelChassis : AbstractChassis
{
public float ThConsistentThreshold = 1.0f;
public float ControlPointRadius = 500f;
public float ControlPointRadius = 500f;
// 原地旋转纠偏钳位：SendRotateMotion 中每轮纠偏速度幅值不超过 该比例×本轮旋转切向速度，
// 防止减速末段旋转切向变小时纠偏盖过它、使轮向矢量乱摆（频繁打方向/卡死）。<0 关闭钳位。
public float RotateCompTangentFrac = 0.5f;
// 原地旋转纠偏钳位：SendRotateMotion 中每轮纠偏速度幅值不超过 该比例×本轮旋转切向速度，
// 防止减速末段旋转切向变小时纠偏盖过它、使轮向矢量乱摆（频繁打方向/卡死）。<0 关闭钳位。
public float RotateCompTangentFrac = 0.5f;
// 上一次 SendRotateMotion 的舵轮对齐状态：false 表示舵轮未追上目标角(gate=0、车未真正转动)。
// 供上层做积分抗饱和（卡死时冻结积分）。
public bool LastRotateAligned { get; private set; } = true;
// 上一次 SendRotateMotion 的舵轮对齐状态：false 表示舵轮未追上目标角(gate=0、车未真正转动)。
// 供上层做积分抗饱和（卡死时冻结积分）。
public bool LastRotateAligned { get; private set; } = true;
public string LastMotionDecomposeFailureReason { get; private set; } = "";
@@ -33,34 +33,34 @@ namespace CommonUsage.Chassis
public float MinimumTurningAngleForAckermann = 60f;
public MultiWheelChassis() : base()
{
public MultiWheelChassis() : base()
{
}
}
public void AddWheel(SteerWheel wheel)
{
_steerWheels.Add(wheel);
}
public void AddWheel(SteerWheel wheel)
{
_steerWheels.Add(wheel);
}
public override void Initialize()
{
_steerWheels = _steerWheels.OrderByDescending(sw => sw.Position.X).ToList();
_targetSpeeds = new float[_steerWheels.Count].ToList();
public override void Initialize()
{
_steerWheels = _steerWheels.OrderByDescending(sw => sw.Position.X).ToList();
_targetSpeeds = new float[_steerWheels.Count].ToList();
_sendSpeeds = new float[_steerWheels.Count].ToList();
_tmpSpeeds = new float[_steerWheels.Count].ToList();
_wheelDirs = Enumerable.Repeat(1, _steerWheels.Count).ToList();
_sendAngle = new float[_steerWheels.Count].ToList();
_sendAngle = new float[_steerWheels.Count].ToList();
_debugSpeeds = new float[_steerWheels.Count].ToList();
if (_steerWheels.Count < 2)
{
Console.WriteLine($"steer wheel num: {_steerWheels.Count}. invalid!");
Valid = false;
return;
}
{
Console.WriteLine($"steer wheel num: {_steerWheels.Count}. invalid!");
Valid = false;
return;
}
CalculateAxes();
CalculateAxes();
Valid = true;
}
@@ -109,7 +109,7 @@ namespace CommonUsage.Chassis
}
public override void AfterDirectionChanged()
{
{
Hedingben.ToastText($"AfterDirectionChanged", "MultiWheelChassis-AfterDirectionChanged");
foreach (var sw in _steerWheels)
@@ -162,26 +162,26 @@ namespace CommonUsage.Chassis
private float _lastGcpTheta1;
/// <summary>
/// 转速单位为度/s，逆时针为正。
/// </summary>
/// <param name="rotSpeed"></param>
public override bool ComputeRotateWheels(float rotSpeed)
{
if (!RotatingActive) ResetMotionState();
/// <summary>
/// 转速单位为度/s，逆时针为正。
/// </summary>
/// <param name="rotSpeed"></param>
public override bool ComputeRotateWheels(float rotSpeed)
{
if (!RotatingActive) ResetMotionState();
var feasible = SendRotateMotion(rotSpeed);
var feasible = SendRotateMotion(rotSpeed);
GoingActive = false;
RotatingActive = true;
XYThActive = false;
return feasible;
}
GoingActive = false;
RotatingActive = true;
XYThActive = false;
return feasible;
}
public override void PredefinedDriveStop()
{
public override void PredefinedDriveStop()
{
if (!Valid) return;
for (var i = 0; i < _steerWheels.Count; i++)
for (var i = 0; i < _steerWheels.Count; i++)
{
_sendSpeeds[i] = 0;
_debugSpeeds[i] = 0;
@@ -195,10 +195,10 @@ namespace CommonUsage.Chassis
_steerWheels[i].WriteSpeed(0);
}
}
GoingActive = false;
RotatingActive = false;
XYThActive = false;
}
GoingActive = false;
RotatingActive = false;
XYThActive = false;
}
public void RampStop(TimeSpan? deltaTime = null)
{
@@ -216,6 +216,48 @@ namespace CommonUsage.Chassis
LastMoveTime = DateTime.Now;
}
/// <summary>
/// 立即清零XYTh驱动轮速度，同时保留已经准备好的舵角目标和轮速方向。
/// 下一条非零命令仍需重新确认四轮实际舵角到位后才会开放驱动速度。
/// </summary>
public void StopXYThDrivePreserveSteeringState()
{
if (!Valid) return;
// 只有已完成Prepare/Adopt交接的XYTh模式才能保留状态。
if (!XYThActive)
{
PredefinedDriveStop();
return;
}
for (var i = 0; i < _steerWheels.Count; i++)
{
_targetSpeeds[i] = 0;
_sendSpeeds[i] = 0;
_debugSpeeds[i] = 0;
if (_steerWheels[i] is DiffSteerWheel diffSteerWheel)
{
diffSteerWheel.WriteLeftSpeed(0);
diffSteerWheel.WriteRightSpeed(0);
}
else
{
_steerWheels[i].WriteSpeed(0);
}
}
GoingActive = false;
RotatingActive = false;
// 保留XYThActive、_sendAngle和_wheelDirs，避免重新选择等价舵角；
// 清除到位标记，使下一次推动摇杆时重新核对实际反馈。
_xyThWheelsAligned = false;
LastMoveTime = DateTime.Now;
LastMotionDecomposeFailureReason = "";
}
private struct WheelAngleCandidate
{
public bool Valid;
@@ -393,8 +435,8 @@ namespace CommonUsage.Chassis
/// <exception cref="Exception"></exception>
public bool SendMotion(float speed, float frontTh, float rearTh, TimeSpan? deltaTime = null,
float localControlRadius = 0, float localCompensateX = 0f, float localCompensateY = 0, float localCompensateTh = 0)
{
if (!Valid) return FailMotionDecomposition("SendMotion", "invalid chassis", deltaTime);
{
if (!Valid) return FailMotionDecomposition("SendMotion", "invalid chassis", deltaTime);
LastMotionDecomposeFailureReason = "";
var compInputSpeed = speed;
@@ -463,11 +505,11 @@ namespace CommonUsage.Chassis
}
float Interpolate(float left, float current, float right)
{
if (current <= left) return 0;
if (current >= right) return 1;
return (current - left) / (right - left);
}
{
if (current <= left) return 0;
if (current >= right) return 1;
return (current - left) / (right - left);
}
while (needAdjust && scaleFactor > minScaleFactor)
{
@@ -477,7 +519,7 @@ namespace CommonUsage.Chassis
// ... 现有的插值函数和旋转中心计算代码 ...
var thDiff = CommonMath.ThDiff(currentFrontTh, currentRearTh);
axisDiffFlag = Math.Abs(thDiff) > ThConsistentThreshold;
var sign = -1;
if (axisDiffFlag)
{
@@ -641,14 +683,14 @@ namespace CommonUsage.Chassis
}
var speedBeforeAlignGate = speed;
if (!GoingWheelAligned)
{
if (!GoingWheelAligned)
{
Hedingben.ToastText("Wheel Not Aligned", "MultiWheelChassis-SendMotion-notAligned");
speed = 0;
var aligned = _steerWheels.Select((sw, i) => (sw, i)).All(ww =>
Math.Abs(CommonMath.ThDiff(ww.sw.ReadAngle(), _sendAngle[ww.i])) < 1);
GoingWheelAligned = aligned;
}
speed = 0;
var aligned = _steerWheels.Select((sw, i) => (sw, i)).All(ww =>
Math.Abs(CommonMath.ThDiff(ww.sw.ReadAngle(), _sendAngle[ww.i])) < 1);
GoingWheelAligned = aligned;
}
for (var i = 0; i < _steerWheels.Count; i++)
{
@@ -693,99 +735,168 @@ namespace CommonUsage.Chassis
return true;
}
/// <summary>
/// 停车并将四个舵轮转到绕当前坐标原点自转所需的切线方向。
/// 只下发舵角，不下发驱动速度。
/// </summary>
public bool PrepareRotateWheels(float alignmentToleranceDegrees = 2.0f)
{
if (!Valid)
return FailMotionDecomposition(
"PrepareRotateWheels",
"invalid chassis",
null);
/// <summary>
/// 停车并将四个舵轮转到绕当前坐标原点自转所需的切线方向。
/// 只下发舵角，不下发驱动速度。
/// </summary>
public bool PrepareRotateWheels(float alignmentToleranceDegrees = 2.0f)
{
if (!Valid)
return FailMotionDecomposition(
"PrepareRotateWheels",
"invalid chassis",
null);
if (float.IsNaN(alignmentToleranceDegrees) ||
float.IsInfinity(alignmentToleranceDegrees) ||
alignmentToleranceDegrees < 0.0f)
throw new ArgumentOutOfRangeException(
nameof(alignmentToleranceDegrees),
"自转舵轮到位容差必须是非负有限值。");
if (float.IsNaN(alignmentToleranceDegrees) ||
float.IsInfinity(alignmentToleranceDegrees) ||
alignmentToleranceDegrees < 0.0f)
throw new ArgumentOutOfRangeException(
nameof(alignmentToleranceDegrees),
"自转舵轮到位容差必须是非负有限值。");
if (_steerWheels.Count == 0)
return FailMotionDecomposition(
"PrepareRotateWheels",
"no steer wheels",
null);
if (_steerWheels.Count == 0)
return FailMotionDecomposition(
"PrepareRotateWheels",
"no steer wheels",
null);
// 模式切换期间必须保持驱动轮停止。
PredefinedDriveStop();
// 模式切换期间必须保持驱动轮停止。
PredefinedDriveStop();
var targetAngles = new float[_steerWheels.Count];
var directions = new int[_steerWheels.Count];
var targetAngles = new float[_steerWheels.Count];
var directions = new int[_steerWheels.Count];
// 先完成全部舵角解算，再统一下发，避免只转动部分舵轮。
for (var i = 0; i < _steerWheels.Count; i++)
{
var wheel = _steerWheels[i];
var px = (double)wheel.Position.X;
var py = (double)wheel.Position.Y;
// 先完成全部舵角解算，再统一下发，避免只转动部分舵轮。
for (var i = 0; i < _steerWheels.Count; i++)
{
var wheel = _steerWheels[i];
var px = (double)wheel.Position.X;
var py = (double)wheel.Position.Y;
// 逆时针绕原点旋转时，该舵轮的切向方向为(-py, px)。
var tangentDegrees =
(float)(Math.Atan2(px, -py) /
Math.PI * 180.0);
tangentDegrees = CommonMath.ThDiff(
tangentDegrees,
wheel.ZeroDirection);
// 逆时针绕原点旋转时，该舵轮的切向方向为(-py, px)。
var tangentDegrees =
(float)(Math.Atan2(px, -py) /
Math.PI * 180.0);
tangentDegrees = CommonMath.ThDiff(
tangentDegrees,
wheel.ZeroDirection);
if (!TryResolveWheelAngle(
i,
tangentDegrees,
"PrepareRotateWheels",
out targetAngles[i],
out directions[i],
out var reason))
return FailMotionDecomposition(
"PrepareRotateWheels",
reason,
null);
}
if (!TryResolveWheelAngle(
i,
tangentDegrees,
"PrepareRotateWheels",
out targetAngles[i],
out directions[i],
out var reason))
return FailMotionDecomposition(
"PrepareRotateWheels",
reason,
null);
}
for (var i = 0; i < _steerWheels.Count; i++)
{
_wheelDirs[i] = directions[i];
SendTh(i, targetAngles[i]);
}
for (var i = 0; i < _steerWheels.Count; i++)
{
_wheelDirs[i] = directions[i];
SendTh(i, targetAngles[i]);
}
var allAligned = true;
for (var i = 0; i < _steerWheels.Count; i++)
{
var actualAngle = _steerWheels[i].ReadAngle();
var angleError = targetAngles[i] - actualAngle;
var allAligned = true;
for (var i = 0; i < _steerWheels.Count; i++)
{
var actualAngle = _steerWheels[i].ReadAngle();
var angleError = targetAngles[i] - actualAngle;
// 这里比较受机械限位约束的真实舵角，不能使用圆周最短角度差。
if (float.IsNaN(actualAngle) ||
float.IsInfinity(actualAngle) ||
Math.Abs(angleError) > alignmentToleranceDegrees)
allAligned = false;
}
// 这里比较受机械限位约束的真实舵角，不能使用圆周最短角度差。
if (float.IsNaN(actualAngle) ||
float.IsInfinity(actualAngle) ||
Math.Abs(angleError) > alignmentToleranceDegrees)
allAligned = false;
}
LastRotateAligned = allAligned;
LastMotionDecomposeFailureReason = "";
return true;
}
LastRotateAligned = allAligned;
LastMotionDecomposeFailureReason = "";
return true;
}
/// <summary>
/// 绕"已被 SetOriginBias 偏置到车队中心的原点"做原地旋转，可叠加一个车体系小幅纠偏旋量。
/// </summary>
/// <param name="rotSpeed">绕车队中心角速度(deg/s，逆时针为正)。</param>
/// <param name="localCompensateX">车体系纵向(前+)修正速度(mm/s)，多车联动维持队形用。</param>
/// <param name="localCompensateY">车体系横向(左+)修正速度(mm/s)。</param>
/// <param name="localCompensateTh">绕本车几何中心附加角速度(deg/s)，修正朝向偏差。</param>
public bool SendRotateMotion(float rotSpeed, TimeSpan? deltaTime = null,
float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f)
{
/// <summary>
/// 将PrepareRotateWheels已经确认到位的舵角和轮速方向，
/// 原样交接给SendXYThSpeed，作为一段XYTh运动的初始状态。
/// 该方法不会调用ResetMotionState，因此不会重新选择等价舵角。
/// </summary>
public bool AdoptPreparedRotateWheelsForXYTh(
float alignmentToleranceDegrees = 2.0f)
{
if (!Valid)
return FailMotionDecomposition(
"AdoptPreparedRotateWheelsForXYTh",
"invalid chassis",
null);
if (float.IsNaN(alignmentToleranceDegrees) ||
float.IsInfinity(alignmentToleranceDegrees) ||
alignmentToleranceDegrees < 0.0f)
throw new ArgumentOutOfRangeException(
nameof(alignmentToleranceDegrees),
"自转舵轮交接容差必须是非负有限值。");
if (!LastRotateAligned)
return FailMotionDecomposition(
"AdoptPreparedRotateWheelsForXYTh",
"rotate wheels have not been prepared and aligned",
null);
for (var i = 0; i < _steerWheels.Count; i++)
{
var actualAngle =
_steerWheels[i].ReadAngle();
if (float.IsNaN(actualAngle) ||
float.IsInfinity(actualAngle))
{
LastRotateAligned = false;
return FailMotionDecomposition(
"AdoptPreparedRotateWheelsForXYTh",
$"wheel {i} angle feedback is invalid: {actualAngle}",
null);
}
// 比较受机械限位约束的实际舵角，不使用圆周最短角。
var angleError =
_sendAngle[i] - actualAngle;
if (Math.Abs(angleError) >
alignmentToleranceDegrees)
{
LastRotateAligned = false;
return FailMotionDecomposition(
"AdoptPreparedRotateWheelsForXYTh",
$"wheel {i} is no longer aligned: " +
$"target={_sendAngle[i]:F1}, actual={actualAngle:F1}, " +
$"error={angleError:F1}",
null);
}
}
// 直接继承PrepareRotateWheels写入的_wheelDirs和_sendAngle。
// 下一次SendXYThSpeed调用看到XYThActive=true时不会重置这些状态。
XYThActive = true;
_xyThWheelsAligned = true;
GoingActive = false;
RotatingActive = false;
LastMoveTime = DateTime.Now;
LastMotionDecomposeFailureReason = "";
return true;
}
/// <summary>
/// 绕"已被 SetOriginBias 偏置到车队中心的原点"做原地旋转，可叠加一个车体系小幅纠偏旋量。
/// </summary>
/// <param name="rotSpeed">绕车队中心角速度(deg/s，逆时针为正)。</param>
/// <param name="localCompensateX">车体系纵向(前+)修正速度(mm/s)，多车联动维持队形用。</param>
/// <param name="localCompensateY">车体系横向(左+)修正速度(mm/s)。</param>
/// <param name="localCompensateTh">绕本车几何中心附加角速度(deg/s)，修正朝向偏差。</param>
public bool SendRotateMotion(float rotSpeed, TimeSpan? deltaTime = null,
float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f)
{
if (!Valid) return FailMotionDecomposition("SendRotateMotion", "invalid chassis", deltaTime);
LastMotionDecomposeFailureReason = "";
if (Math.Abs(rotSpeed) < 1e-6f &&
@@ -798,10 +909,10 @@ namespace CommonUsage.Chassis
return true;
}
var ths = new float[_steerWheels.Count];
var dirs = new int[_steerWheels.Count];
// 每轮合速度大小(m/s)，含"绕队心旋转 + 车体平移纠偏 + 绕本车中心微转纠偏"三项矢量和。
var speedMags = new float[_steerWheels.Count];
var ths = new float[_steerWheels.Count];
var dirs = new int[_steerWheels.Count];
// 每轮合速度大小(m/s)，含"绕队心旋转 + 车体平移纠偏 + 绕本车中心微转纠偏"三项矢量和。
var speedMags = new float[_steerWheels.Count];
var allWheelAligned = true;
// 把车体系纠偏旋量换算到 sw.Position 所在的偏置帧 F(原点=车队中心, 朝向随 _originBiasTh)。
@@ -816,9 +927,9 @@ namespace CommonUsage.Chassis
var biasX = (double)_originBiasX; // 本车几何中心在 F 中的位置
var biasY = (double)_originBiasY;
for (var i = 0; i < _steerWheels.Count; i++)
{
var sw = _steerWheels[i];
for (var i = 0; i < _steerWheels.Count; i++)
{
var sw = _steerWheels[i];
var px = (double)sw.Position.X;
var py = (double)sw.Position.Y;
// 合成轮速矢量(mm/s, F帧)：v = ω_rot ẑ×p + [R(bias)·v_comp + ω_comp ẑ×(p - 本车中心)]
@@ -856,10 +967,10 @@ namespace CommonUsage.Chassis
out var resolveReason))
return FailMotionDecomposition("SendRotateMotion", resolveReason, deltaTime);
_wheelDirs[i] = dirs[i];
}
}
for (var i = 0; i < _steerWheels.Count; ++i)
SendTh(i, ths[i]);
for (var i = 0; i < _steerWheels.Count; ++i)
SendTh(i, ths[i]);
var slowFac = 1f;
var maxDth = 0f;
for (var i = 0; i < _steerWheels.Count; ++i)
@@ -907,16 +1018,16 @@ namespace CommonUsage.Chassis
private DateTime _rotDbgLast = DateTime.MinValue;
public override float CalculateTurningSpeedDecayFac(float turn)
{
return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac;
}
public override float CalculateTurningSpeedDecayFac(float turn)
{
return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac;
}
[Obsolete]
public List<SteerWheel> GetSteerWheels()
{
return _steerWheels.ToList();
}
public List<SteerWheel> GetSteerWheels()
{
return _steerWheels.ToList();
}
private void SendTh(int i, float targetTh)
{
@@ -951,13 +1062,13 @@ namespace CommonUsage.Chassis
return (0, rangeFront, rangeRear);
}
private void AccumulateSpeed(int i, float v, bool axisDiff, Vector2 rotCenter, TimeSpan? deltaTime = null)
{
_targetSpeeds[i] = v;
var speedSign = Math.Sign(_targetSpeeds[i] - _sendSpeeds[i]);
var acc = Math.Abs(_targetSpeeds[i]) > Math.Abs(_sendSpeeds[i]) ? AccPerSecond : DeAccPerSecond;
_sendSpeeds[i] += speedSign * Math.Min(Math.Abs(_targetSpeeds[i] - _sendSpeeds[i]),
acc * (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds);
private void AccumulateSpeed(int i, float v, bool axisDiff, Vector2 rotCenter, TimeSpan? deltaTime = null)
{
_targetSpeeds[i] = v;
var speedSign = Math.Sign(_targetSpeeds[i] - _sendSpeeds[i]);
var acc = Math.Abs(_targetSpeeds[i]) > Math.Abs(_sendSpeeds[i]) ? AccPerSecond : DeAccPerSecond;
_sendSpeeds[i] += speedSign * Math.Min(Math.Abs(_targetSpeeds[i] - _sendSpeeds[i]),
acc * (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds);
if (_steerWheels[i] is DiffSteerWheel dsw)
{
if (axisDiff)
@@ -981,76 +1092,76 @@ namespace CommonUsage.Chassis
_steerWheels[i].WriteSpeed(_sendSpeeds[i]);
}
if (Debug)
Console.WriteLine($"Ackermann wheel{i}: target:{_targetSpeeds[i]:0.00},send:{_sendSpeeds[i]:0.0}");
}
if (Debug)
Console.WriteLine($"Ackermann wheel{i}: target:{_targetSpeeds[i]:0.00},send:{_sendSpeeds[i]:0.0}");
}
private void CalculateAxes()
{
var axes = _steerWheels
.Select(sw => (sw, Vector2.Dot(sw.Position, new Vector2(1, 0))))
.OrderByDescending(ax => ax.Item2).ToList();
_wheelBases = axes.Select(ax => ax.Item2).ToList();
_steerWheels = axes.Select(ax => ax.sw).ToList();
GeometricControlPoints = new List<GeometricControlPoint>()
{
new (new Vector2(ControlPointRadius, 0)),
new (new Vector2(-ControlPointRadius, 0))
};
_frontBase = _wheelBases.First();
_rearBase = _wheelBases.Last();
}
private void CalculateAxes()
{
var axes = _steerWheels
.Select(sw => (sw, Vector2.Dot(sw.Position, new Vector2(1, 0))))
.OrderByDescending(ax => ax.Item2).ToList();
_wheelBases = axes.Select(ax => ax.Item2).ToList();
_steerWheels = axes.Select(ax => ax.sw).ToList();
GeometricControlPoints = new List<GeometricControlPoint>()
{
new (new Vector2(ControlPointRadius, 0)),
new (new Vector2(-ControlPointRadius, 0))
};
_frontBase = _wheelBases.First();
_rearBase = _wheelBases.Last();
}
private List<SteerWheel> _steerWheels = new ();
private float _frontBase, _rearBase;
private List<float> _wheelBases;
private List<float> _sendSpeeds;
private List<float> _targetSpeeds;
private List<float> _sendAngle;
private List<SteerWheel> _steerWheels = new();
private float _frontBase, _rearBase;
private List<float> _wheelBases;
private List<float> _sendSpeeds;
private List<float> _targetSpeeds;
private List<float> _sendAngle;
private List<float> _debugSpeeds;
private DateTime _sendMotionDetailLastLog = DateTime.MinValue;
private DateTime _geometricComputeLastLog = DateTime.MinValue;
private List<float> _tmpSpeeds;
// add TimeStamp, prevent the wheel from swaying.
// for example, target angle is 90, current wheel is around 0. if no TimeStamp,
// wheel will swing between 90 and -90.
private List<int> _wheelDirs;
private List<float> _tmpSpeeds;
// add TimeStamp, prevent the wheel from swaying.
// for example, target angle is 90, current wheel is around 0. if no TimeStamp,
// wheel will swing between 90 and -90.
private List<int> _wheelDirs;
// call this before a new continuous motion happens
private void ResetMotionState()
{
LastMoveTime = DateTime.Now;
GoingWheelAligned = false;
_wheelDirs = Enumerable.Repeat(1, _steerWheels.Count).ToList();
}
// call this before a new motion sequence happens
private void ResetMotionState()
{
LastMoveTime = DateTime.Now;
GoingWheelAligned = false;
_wheelDirs = Enumerable.Repeat(1, _steerWheels.Count).ToList();
}
public void AddTestFunction()
{
}
/// <summary>
/// 得到相对舵轮在车体坐标系下的分解速度
/// </summary>
/// <param name="pos"></param>
/// <param name="vx"></param>
/// <param name="vy"></param>
/// <param name="vth">单位为°/s</param>
/// <returns></returns>
/// <summary>
/// 得到相对舵轮在车体坐标系下的分解速度
/// </summary>
/// <param name="pos"></param>
/// <param name="vx"></param>
/// <param name="vy"></param>
/// <param name="vth">单位为°/s</param>
/// <returns></returns>
private Vector2 VectorVelocity(Vector2 pos, float vx, float vy, float vth, int i)
{
var vRotX = -vth / 180 * (float)Math.PI * pos.Y / 1000;
var vRotY = vth / 180 * (float)Math.PI * pos.X / 1000;
return new Vector2(vx + vRotX, vy + vRotY);
}
/// <summary>
/// 获得车轮应该打的角度和速度
/// </summary>
/// <param name="pos"></param>
/// <param name="vx"></param>
/// <param name="vy"></param>
/// <param name="vth">单位为°/s</param>
/// <returns></returns>
/// <summary>
/// 获得车轮应该打的角度和速度
/// </summary>
/// <param name="pos"></param>
/// <param name="vx"></param>
/// <param name="vy"></param>
/// <param name="vth">单位为°/s</param>
/// <returns></returns>
private (float angle, float speed) AngleAndSpeed(Vector2 pos, float vx, float vy, float vth, int i)
{
var v = VectorVelocity(pos, vx, vy, vth, i);
@@ -1061,13 +1172,33 @@ namespace CommonUsage.Chassis
/// 原地旋转时舵角误差对应的速度衰减宽度，单位为度。
/// </summary>
public float SteeringAlignmentSigmaDegrees { get; set; } = 8f;
/// <summary>
/// 单轮速度低于此值时认为其运动方向无意义，单位为m/s。
/// </summary>
public float WheelDirectionDeadbandMetersPerSecond { get; set; } = 0.005f;
private bool XYThActive = false;
private bool _xyThWheelsAligned = false;
private DateTime _xyThDiagnosticsLastTime = DateTime.MinValue;
public bool SendXYThSpeed(float vx, float vy, float vth, TimeSpan? deltaTime = null)
/// <summary>
/// 下发车体二维速度，并根据舵轮机械角度误差进行高斯降速。
/// 一段运动开始时必须先等待全部舵轮到位；运动过程中舵角误差越大，
/// 四轮驱动速度的统一缩放比例越小，适合作为默认安全接口。
/// vx、vy单位为m/s，vth单位为°/s。
/// </summary>
public bool SendXYThSpeed(
float vx,
float vy,
float vth,
TimeSpan? deltaTime = null)
{
if (!Valid) return FailMotionDecomposition("SendXYThSpeed", "invalid chassis", deltaTime);
const string operationName = "SendXYThSpeed";
if (!Valid)
return FailMotionDecomposition(
operationName,
"invalid chassis",
deltaTime);
if (Math.Abs(vx) < 1e-6f && Math.Abs(vy) < 1e-6f && Math.Abs(vth) < 1e-6f)
{
@@ -1080,18 +1211,19 @@ namespace CommonUsage.Chassis
return true;
}
if (!XYThActive)
if (!XYThActive)
{
ResetMotionState();
_xyThWheelsAligned = false;
}
XYThActive = true;
XYThActive = true;
GoingActive = false;
RotatingActive = false;
float[] sendSpeed = new float[_steerWheels.Count];
var allWheelsAligned = true;
var maximumAngleError = 0f;
var alignmentSpeedScale = 1f;
const float initialAlignmentToleranceDegrees = 2f;
var writeDiagnostics =
Debug &&
@@ -1102,31 +1234,59 @@ namespace CommonUsage.Chassis
{
var sw = _steerWheels[i];
var (angle, speed) = AngleAndSpeed(sw.Position, vx, vy, vth, i);
// 单轮合成速度接近零时，运动方向没有物理意义。
// 此时不重新计算和下发舵角，保持上一目标舵角，轮速降为零。
var directionDeadband = Math.Max(
WheelDirectionDeadbandMetersPerSecond,
0f);
if (speed < directionDeadband)
{
sendSpeed[i] = 0f;
if (writeDiagnostics)
{
Hedingben.ToastText(
$"hold-angle speed:{speed:F4} deadband:{directionDeadband:F4}",
$"{operationName}-{i}");
}
continue;
}
var actualTh = sw.ReadAngle();
if (float.IsNaN(actualTh) ||
float.IsInfinity(actualTh))
{
return FailMotionDecomposition(
"SendXYThSpeed",
operationName,
$"wheel {i} angle feedback is invalid: {actualTh}",
deltaTime);
}
if (!TryResolveWheelAngle(i, CommonMath.ThDiff(angle, sw.ZeroDirection), "SendXYThSpeed",
if (!TryResolveWheelAngle(i, CommonMath.ThDiff(angle, sw.ZeroDirection), operationName,
out var useAngle, out var dir, out var resolveReason))
return FailMotionDecomposition("SendXYThSpeed", resolveReason, deltaTime);
return FailMotionDecomposition(operationName, resolveReason, deltaTime);
speed *= dir;
_wheelDirs[i] = dir;
sendSpeed[i] = speed;
if (speed!=0)
SendTh(i, useAngle);
SendTh(i, useAngle);
// 这里比较受机械限位约束的实际舵角，不使用圆周最短角。
var angleError =
Math.Abs(_sendAngle[i] - actualTh);
maximumAngleError =
Math.Max(maximumAngleError, angleError);
alignmentSpeedScale = Math.Min(
alignmentSpeedScale,
CommonMath.gaussmf(
angleError,
Math.Max(
SteeringAlignmentSigmaDegrees,
0.1f),
0));
if (angleError >
initialAlignmentToleranceDegrees)
{
@@ -1136,37 +1296,40 @@ namespace CommonUsage.Chassis
if (writeDiagnostics)
{
Hedingben.ToastText(
$"ready:{_xyThWheelsAligned} err:{angleError:F1} " +
$"ready:{_xyThWheelsAligned} " +
$"err:{angleError:F1} scale:{alignmentSpeedScale:F2} " +
$"s:{speed:F3} th:{_sendAngle[i]:F1} actualTh:{actualTh:F1}",
$"SendXYThSpeed-{i}");
$"{operationName}-{i}");
}
}
// 仅在一段XYTh运动刚开始时等待舵轮到位。
// 连续运动开始后，正常改变vx/vy/vth时允许舵轮边转、车辆边走，
// 避免每次打方向都重新把驱动速度压到零。
// 一段XYTh运动刚开始时必须等待全部舵轮到位。
if (!_xyThWheelsAligned &&
allWheelsAligned)
{
_xyThWheelsAligned = true;
}
var driveEnabled = _xyThWheelsAligned;
var driveScale = _xyThWheelsAligned
? alignmentSpeedScale
: 0f;
for (var i = 0; i < _steerWheels.Count; i++)
AccumulateSpeed(
i,
driveEnabled ? sendSpeed[i] : 0f,
driveScale *
sendSpeed[i],
false,
new Vector2(0f,0f),
new Vector2(0f, 0f),
deltaTime);
if (writeDiagnostics)
{
_xyThDiagnosticsLastTime = DateTime.Now;
Hedingben.ToastText(
$"ready:{_xyThWheelsAligned} maxErr:{maximumAngleError:F1} " +
$"ready:{_xyThWheelsAligned} " +
$"maxErr:{maximumAngleError:F1} scale:{driveScale:F2} " +
$"cmd:({vx:F3},{vy:F3},{vth:F1})",
"SendXYThSpeed-alignment");
$"{operationName}-alignment");
}
//todo 计算rotCenter填入
LastMoveTime = DateTime.Now;
@@ -1183,7 +1346,7 @@ namespace CommonUsage.Chassis
{
var sw1 = _steerWheels[i];
Vector2 a = sw1.Position / 1000;
//var tha = _sendAngle[i] / 180 * (float)Math.PI;
//var tha = _sendAngle[i] / 180 * (float)Math.PI;
var tha = isActual ? sw1.ReadAngle() / 180 * (float)Math.PI : _sendAngle[i] / 180 * (float)Math.PI;
//var speeda = _sendSpeeds[i];
//var speeda = isActual ? sw1.ReadSpeed() : _sendSpeeds[i];
@@ -1195,7 +1358,7 @@ namespace CommonUsage.Chassis
{
var sw2 = _steerWheels[j];
Vector2 b = sw2.Position / 1000;
//var thb = _sendAngle[j] / 180 * (float)Math.PI;
//var thb = _sendAngle[j] / 180 * (float)Math.PI;
var thb = isActual ? sw2.ReadAngle() / 180 * (float)Math.PI : _sendAngle[j] / 180 * (float)Math.PI;
//var speedb = _sendSpeeds[j];
//var speedb = isActual ? sw2.ReadSpeed() : _sendSpeeds[j];
@@ -1208,7 +1371,7 @@ namespace CommonUsage.Chassis
}
}
return new CarSpeed()
{ Vx = vx.Average(), Vy = vy.Average(), Vw = (float)(vth.Average() / Math.PI * 180f) };
{ Vx = vx.Average(), Vy = vy.Average(), Vw = (float)(vth.Average() / Math.PI * 180f) };
}
private static (float, float, float) CenterVelocityFromPoints(Vector2 a, Vector2 va, Vector2 b, Vector2 vb)