增加差速电机前馈与控制器曲率预瞄
This commit is contained in:
+165
-12
@@ -3,14 +3,22 @@ using CartActivator;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Commons;
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public class MotorRoutine : LadderLogic<DiverCartDefinition>
|
||||
{
|
||||
private const double MaximumFeedforwardIntervalSeconds = 0.2;
|
||||
private bool _wasTransmitterControlling;
|
||||
private DateTime _lastMoveTime = DateTime.Now;
|
||||
private bool _diffSteerFeedforwardInitialized;
|
||||
private long _lastDiffSteerFeedforwardTimestamp;
|
||||
private float _previousThLeftFront;
|
||||
private float _previousThLeftRear;
|
||||
private float _previousThRightFront;
|
||||
private float _previousThRightRear;
|
||||
private DiverCartDefinition.ManualControlMode?
|
||||
_pendingTransmitterControlMode;
|
||||
private DateTime _pendingTransmitterControlModeSince =
|
||||
@@ -213,7 +221,9 @@ namespace MedullaAdapter
|
||||
cart.SpeedRightArm = 0;
|
||||
}
|
||||
|
||||
// M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
|
||||
/// <summary>
|
||||
/// 根据四个舵轮的目标角速度前馈和实际角度反馈修正八个驱动电机速度。
|
||||
/// </summary>
|
||||
private void UpdateDiffSteerWheelSpeeds()
|
||||
{
|
||||
if (cart.LeftFrontPid == null ||
|
||||
@@ -229,6 +239,7 @@ namespace MedullaAdapter
|
||||
cart.SpeedLRR = 0;
|
||||
cart.SpeedRRL = 0;
|
||||
cart.SpeedRRR = 0;
|
||||
ResetDiffSteerRateFeedforward();
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -272,26 +283,45 @@ namespace MedullaAdapter
|
||||
cart.DiffSteerThresh,
|
||||
cart.DiffSteerSpeedAcc);
|
||||
|
||||
// 根据实际舵角计算四条腿的差速修正量。
|
||||
var diffLf = cart.LeftFrontPid.GetResponse(
|
||||
// 根据实际舵角计算四条腿的PID反馈修正量。
|
||||
var feedbackLf = cart.LeftFrontPid.GetResponse(
|
||||
cart.ThLeftFront, false, false, "LF");
|
||||
|
||||
var diffLr = cart.LeftRearPid.GetResponse(
|
||||
var feedbackLr = cart.LeftRearPid.GetResponse(
|
||||
cart.ThLeftRear, false, false, "LR");
|
||||
|
||||
var diffRf = cart.RightFrontPid.GetResponse(
|
||||
var feedbackRf = cart.RightFrontPid.GetResponse(
|
||||
cart.ThRightFront, false, false, "RF");
|
||||
|
||||
var diffRr = cart.RightRearPid.GetResponse(
|
||||
var feedbackRr = cart.RightRearPid.GetResponse(
|
||||
cart.ThRightRear, false, false, "RR");
|
||||
|
||||
// 保存四个转向PID的本周期修正量，供M层监控和舵轮响应CSV记录使用。
|
||||
cart.DiffSteerOutputLeftFront = diffLf;
|
||||
cart.DiffSteerOutputLeftRear = diffLr;
|
||||
cart.DiffSteerOutputRightFront = diffRf;
|
||||
cart.DiffSteerOutputRightRear = diffRr;
|
||||
CalculateDiffSteerRateFeedforward(
|
||||
out var feedforwardLf,
|
||||
out var feedforwardLr,
|
||||
out var feedforwardRf,
|
||||
out var feedforwardRr);
|
||||
|
||||
// 左前腿：左右电机施加方向相反的PID修正量。
|
||||
var diffLf = feedbackLf + feedforwardLf;
|
||||
var diffLr = feedbackLr + feedforwardLr;
|
||||
var diffRf = feedbackRf + feedforwardRf;
|
||||
var diffRr = feedbackRr + feedforwardRr;
|
||||
|
||||
// 分别保留PID、前馈和合成差速，便于独立标定与诊断。
|
||||
cart.DiffSteerOutputLeftFront = feedbackLf;
|
||||
cart.DiffSteerOutputLeftRear = feedbackLr;
|
||||
cart.DiffSteerOutputRightFront = feedbackRf;
|
||||
cart.DiffSteerOutputRightRear = feedbackRr;
|
||||
cart.DiffSteerRateFeedforwardLeftFront = feedforwardLf;
|
||||
cart.DiffSteerRateFeedforwardLeftRear = feedforwardLr;
|
||||
cart.DiffSteerRateFeedforwardRightFront = feedforwardRf;
|
||||
cart.DiffSteerRateFeedforwardRightRear = feedforwardRr;
|
||||
cart.DiffSteerTotalOutputLeftFront = diffLf;
|
||||
cart.DiffSteerTotalOutputLeftRear = diffLr;
|
||||
cart.DiffSteerTotalOutputRightFront = diffRf;
|
||||
cart.DiffSteerTotalOutputRightRear = diffRr;
|
||||
|
||||
// 左前腿：左右电机施加方向相反的合成差速修正量。
|
||||
cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
|
||||
cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
|
||||
|
||||
@@ -308,6 +338,129 @@ namespace MedullaAdapter
|
||||
cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据四个机械目标舵角的实际变化率计算本周期差速轮线速度前馈。
|
||||
/// </summary>
|
||||
private void CalculateDiffSteerRateFeedforward(
|
||||
out float leftFront,
|
||||
out float leftRear,
|
||||
out float rightFront,
|
||||
out float rightRear)
|
||||
{
|
||||
leftFront = 0f;
|
||||
leftRear = 0f;
|
||||
rightFront = 0f;
|
||||
rightRear = 0f;
|
||||
|
||||
var currentTimestamp = Stopwatch.GetTimestamp();
|
||||
|
||||
if (_diffSteerFeedforwardInitialized)
|
||||
{
|
||||
var deltaTimeSeconds =
|
||||
(currentTimestamp -
|
||||
_lastDiffSteerFeedforwardTimestamp) /
|
||||
(double)Stopwatch.Frequency;
|
||||
|
||||
if (deltaTimeSeconds > 0.0 &&
|
||||
deltaTimeSeconds <=
|
||||
MaximumFeedforwardIntervalSeconds)
|
||||
{
|
||||
leftFront = CalculateDiffSteerRateFeedforward(
|
||||
cart.ThLeftFront,
|
||||
_previousThLeftFront,
|
||||
deltaTimeSeconds);
|
||||
leftRear = CalculateDiffSteerRateFeedforward(
|
||||
cart.ThLeftRear,
|
||||
_previousThLeftRear,
|
||||
deltaTimeSeconds);
|
||||
rightFront = CalculateDiffSteerRateFeedforward(
|
||||
cart.ThRightFront,
|
||||
_previousThRightFront,
|
||||
deltaTimeSeconds);
|
||||
rightRear = CalculateDiffSteerRateFeedforward(
|
||||
cart.ThRightRear,
|
||||
_previousThRightRear,
|
||||
deltaTimeSeconds);
|
||||
}
|
||||
}
|
||||
|
||||
_previousThLeftFront = cart.ThLeftFront;
|
||||
_previousThLeftRear = cart.ThLeftRear;
|
||||
_previousThRightFront = cart.ThRightFront;
|
||||
_previousThRightRear = cart.ThRightRear;
|
||||
_lastDiffSteerFeedforwardTimestamp = currentTimestamp;
|
||||
_diffSteerFeedforwardInitialized = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将单个机械目标舵角变化率转换为带限幅的左右轮差速线速度前馈。
|
||||
/// </summary>
|
||||
private float CalculateDiffSteerRateFeedforward(
|
||||
float targetAngleDegrees,
|
||||
float previousTargetAngleDegrees,
|
||||
double deltaTimeSeconds)
|
||||
{
|
||||
var gain = cart.DiffSteerRateFeedforwardGain;
|
||||
var wheelDistanceMillimeters =
|
||||
cart.DiffSteerWheelDistanceMillimeters;
|
||||
var maximumSpeed =
|
||||
cart.DiffSteerRateFeedforwardMaximumSpeed;
|
||||
|
||||
if (!IsFinite(gain) || gain <= 0f ||
|
||||
!IsFinite(wheelDistanceMillimeters) ||
|
||||
wheelDistanceMillimeters <= 0f ||
|
||||
!IsFinite(maximumSpeed) || maximumSpeed <= 0f ||
|
||||
!IsFinite(targetAngleDegrees) ||
|
||||
!IsFinite(previousTargetAngleDegrees))
|
||||
{
|
||||
return 0f;
|
||||
}
|
||||
|
||||
// 机械舵角受限，必须使用直接差值而不是圆周最短角差。
|
||||
var targetRateRadiansPerSecond =
|
||||
(targetAngleDegrees - previousTargetAngleDegrees) *
|
||||
Math.PI / 180.0 /
|
||||
deltaTimeSeconds;
|
||||
var wheelDistanceMeters =
|
||||
wheelDistanceMillimeters / 1000.0;
|
||||
var feedforwardSpeed =
|
||||
0.5 *
|
||||
wheelDistanceMeters *
|
||||
targetRateRadiansPerSecond *
|
||||
gain;
|
||||
|
||||
return (float)Math.Clamp(
|
||||
feedforwardSpeed,
|
||||
-maximumSpeed,
|
||||
maximumSpeed);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清除差速转舵前馈历史和监控输出，避免恢复控制时使用过期目标角。
|
||||
/// </summary>
|
||||
private void ResetDiffSteerRateFeedforward()
|
||||
{
|
||||
_diffSteerFeedforwardInitialized = false;
|
||||
_lastDiffSteerFeedforwardTimestamp = 0;
|
||||
cart.DiffSteerRateFeedforwardLeftFront = 0f;
|
||||
cart.DiffSteerRateFeedforwardLeftRear = 0f;
|
||||
cart.DiffSteerRateFeedforwardRightFront = 0f;
|
||||
cart.DiffSteerRateFeedforwardRightRear = 0f;
|
||||
cart.DiffSteerTotalOutputLeftFront = 0f;
|
||||
cart.DiffSteerTotalOutputLeftRear = 0f;
|
||||
cart.DiffSteerTotalOutputRightFront = 0f;
|
||||
cart.DiffSteerTotalOutputRightRear = 0f;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断单精度参数是否可安全参与底盘控制计算。
|
||||
/// </summary>
|
||||
private static bool IsFinite(float value)
|
||||
{
|
||||
return !float.IsNaN(value) &&
|
||||
!float.IsInfinity(value);
|
||||
}
|
||||
|
||||
// M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
|
||||
private void UpdateSendSpeedLimit()
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user