本文主要梳理Marlin2.0工程代码中关于运动控制部分的理解。Marlin1.0工程代码用C语言写的，阅读起来比较容易。Marlin1.0主要核心算法包括圆弧插补、速度前瞻、转角速度圆滑、梯形速度规划、Bresenham多轴插补。Marlin2.0工程相对于Marlin1.0工程程序用了更多C++的写法，程序写的相对专业(晦涩)，许多人不太适应，其实2.0比1.0主要是增加了S形速度规划。

1 程序主循环 、G代码解析、圆弧插补

程序主循环非常简洁：

void loop() {

for (;;) {

idle(); // Do an idle first so boot is slightly faster

#if ENABLED(SDSUPPORT)

card.checkautostart();

if (card.flag.abort_sd_printing) abortSDPrinting();

#endif

queue.advance();

endstops.event_handler();

}

}

对上位机传过来的G代码解析都在queue.advance()函数中。G0、G1是直线插补命令，G3、G4是圆弧插补命令。源码路径中motion文件夹中G0_G1.cpp的G0_G1()就是解析G0、G1直线插补命令，G2_G3.cpp的G2_G3()就是解析圆弧插补命令。这里看圆弧插补函数

void plan_arc(

const xyze_pos_t &cart,   // Destination position //目标位置

const ab_float_t &offset, // Center of rotation relative to current_position

//相对于当前位current_position的圆心位置，有center_position=current_position+offset

const uint8_t clockwise   // Clockwise? //顺时针还是逆时针插补

)

先列出圆弧插补原理示意图：

圆心坐标O(xc,yc)，起始点Ps(x1,y1),终点Pe(x2,y2)，起始点也是当前点。圆弧插补思想就是算出OPs与OPe的夹角θ，进而求出PsPe段圆弧长度L=rθ，程序设定圆弧插补精度为p，则插补段数为N=L/p，则可以求出第i段的角度为θi=θ1+θ*i/N，则Pi.x=PO.x+r*cos(θs+θi)=PO.x+rcosθscosθi-rsinθssinθi=PO.x+ps.x*cosθi-Ps.y*sinθi，Pi.y=PO.x+r*sin(θs+θi)=PO.x+rsinθscosθi+rcosθssinθi=PO.x+ps.y*cosθi+Ps.x*sinθi，则从Ps到Pe的圆弧插补可以等效于从Ps经一系列中间点P1,P2,.....Pn再到Pe的一系列直线插补。

讲完原理，再来分析代码。

ab_float_t rvec = -offset; //Ps为当前点，O点坐标为(Ps.x+offset.x,Ps.y+offset.y)，则向量OPs=(-offset.x,-offset.y)=-offset。

const float radius = HYPOT(rvec.a, rvec.b),         //计算弧长r,rvec.x=rcosθs,rvec.y=rsinθs

#if ENABLED(AUTO_BED_LEVELING_UBL)

start_L  = current_position[l_axis],

#endif

center_P = current_position[p_axis] - rvec.a,     //圆心坐标，center_P=ps.x+offset.x,center_Q=ps.y+offset.y

center_Q = current_position[q_axis] - rvec.b,

rt_X = cart[p_axis] - center_P,     //计算圆弧终点向量OPe，OPe=Pe-O

rt_Y = cart[q_axis] - center_Q,

linear_travel = cart[l_axis] - current_position[l_axis],

extruder_travel = cart.e - current_position.e;

// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.

float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);//这里用到了向量点积和叉积公式，OPs.OPe=|OPs|*|OPe|*cosθ=OPs.x*OPe.y+OPs.y*OPe.x,OPs X OPe=|OPs|*|OPe|*sinθ=OPs.x*OPe.y-OPs.y*OPe.x

if (angular_travel < 0) angular_travel += RADIANS(360);

#ifdef MIN_ARC_SEGMENTS

uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * (angular_travel / RADIANS(360)));

NOLESS(min_segments, 1U);

#else

constexpr uint16_t min_segments = 1;

#endif

if (clockwise) angular_travel -= RADIANS(360);

// Make a circle if the angular rotation is 0 and the target is current position

if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis]) {

angular_travel = RADIANS(360);

#ifdef MIN_ARC_SEGMENTS

min_segments = MIN_ARC_SEGMENTS;

#endif

}

//求出弧长L=rθ,插补精度为MM_PER_ARC_SEGMENT，则插补总段数N=L/MM_PER_ARC_SEGMENT

const float flat_mm = radius * angular_travel,

mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm);

if (mm_of_travel < 0.001f) return;

uint16_t segments = FLOOR(mm_of_travel / (MM_PER_ARC_SEGMENT));

NOLESS(segments, min_segments);

将N个小圆弧当成直线进行插补：

for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times

........省略代码

const float cos_Ti = cos(i * theta_per_segment),

sin_Ti = sin(i * theta_per_segment);

//计算OPi，OPi=(rcos(θs+θi),rsin(θs+θi)),θi=i*theta_per_segment

rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;

rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;

// Update raw location //Pi的坐标=圆心坐标+OPi的坐标

raw[p_axis] = center_P + rvec.a;

raw[q_axis] = center_Q + rvec.b;

#if ENABLED(AUTO_BED_LEVELING_UBL)

raw[l_axis] = start_L;

UNUSED(linear_per_segment);

#else

raw[l_axis] += linear_per_segment;

#endif

raw.e += extruder_per_segment;

apply_motion_limits(raw);

#if HAS_LEVELING && !PLANNER_LEVELING

planner.apply_leveling(raw);

#endif

//开始执行直线插补，目标点raw

if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT

#if ENABLED(SCARA_FEEDRATE_SCALING)

, inv_duration

#endif

))

break;

}

2 直线规划及速度前瞻算法

直线规划的实现函数在planner.cpp的Planner::buffer_line函数，buffer_line函数又调用buffer_segment函数，

bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e

#if IS_KINEMATIC && DISABLED(CLASSIC_JERK)

, const xyze_float_t &delta_mm_cart

#endif

, const feedRate_t &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/

) {

//调用_buffer_steps进行直线规划，主要是生成一个新的规划block，block中填充初速度、末速度、加速度、加速距离、减速距离等

if (

!_buffer_steps(target

#if HAS_POSITION_FLOAT

, target_float

#endif

#if IS_KINEMATIC && DISABLED(CLASSIC_JERK)

, delta_mm_cart

#endif

, fr_mm_s, extruder, millimeters

)

) return false;

stepper.wake_up();//直线规划完以后唤醒定时器中断，在中断里根据规划的block执行速度规划

return true;

}

_buffer_steps首先调用_populate_block()函数生成新的规划block并进行填充，填充时调用了转角平滑算法来计算初速度，然后再调用recalculate()函数来执行速度前瞻算法和梯形轨迹规划算法。我们先分析_populate_block()函数。

_populate_block()函数

我们来看一下要生成的block结构：

typedef struct block_t {

volatile uint8_t flag; // Block flags (See BlockFlag enum above) - Modified by ISR and main thread!

// Fields used by the motion planner to manage acceleration

float nominal_speed_sqr, // The nominal speed for this block in (mm/sec)^2

entry_speed_sqr, // Entry speed at previous-current junction in (mm/sec)^2

max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/sec)^2

millimeters, // The total travel of this block in mm

acceleration; // acceleration mm/sec^2

union {

abce_ulong_t steps; // Step count along each axis

abce_long_t position; // New position to force when this sync block is executed

};

uint32_t step_event_count; // The number of step events required to complete this block

#if EXTRUDERS > 1

uint8_t extruder; // The extruder to move (if E move)

#else

static constexpr uint8_t extruder = 0;

#endif

#if ENABLED(MIXING_EXTRUDER)

MIXER_BLOCK_FIELD; // Normalized color for the mixing steppers

#endif

// Settings for the trapezoid generator

uint32_t accelerate_until, // The index of the step event on which to stop acceleration

decelerate_after; // The index of the step event on which to start decelerating

#if ENABLED(S_CURVE_ACCELERATION)

uint32_t cruise_rate, // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase

acceleration_time, // Acceleration time and deceleration time in STEP timer counts

deceleration_time,

acceleration_time_inverse, // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used

deceleration_time_inverse;

#else

uint32_t acceleration_rate; // The acceleration rate used for acceleration calculation

#endif

uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)

// Advance extrusion

#if ENABLED(LIN_ADVANCE)

bool use_advance_lead;

uint16_t advance_speed, // STEP timer value for extruder speed offset ISR

max_adv_steps, // max. advance steps to get cruising speed pressure (not always nominal_speed!)

final_adv_steps; // advance steps due to exit speed

float e_D_ratio;

#endif

uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec

initial_rate, // The jerk-adjusted step rate at start of block

final_rate, // The minimal rate at exit

acceleration_steps_per_s2; // acceleration steps/sec^2

#if HAS_CUTTER

cutter_power_t cutter_power; // Power level for Spindle, Laser, etc.

#endif

#if FAN_COUNT > 0

uint8_t fan_speed[FAN_COUNT];

#endif

#if ENABLED(BARICUDA)

uint8_t valve_pressure, e_to_p_pressure;

#endif

#if HAS_SPI_LCD

uint32_t segment_time_us;

#endif

#if ENABLED(POWER_LOSS_RECOVERY)

uint32_t sdpos;

#endif

} block_t;

_populate_block函数就是根据要规划的直线参数生成一个新的规划区块并填充它(有点像区块链)。我们进入_populate_block函数：

/**

* Planner::_populate_block

*

* Fills a new linear movement in the block (in terms of steps).

*

* target - target position in steps units

* fr_mm_s - (target) speed of the move

* extruder - target extruder

*

* Returns true is movement is acceptable, false otherwise

*/

bool Planner::_populate_block(block_t * const block, bool split_move,

const abce_long_t &target

#if HAS_POSITION_FLOAT

, const xyze_pos_t &target_float

#endif

#if IS_KINEMATIC && DISABLED(CLASSIC_JERK)

, const xyze_float_t &delta_mm_cart

#endif

, feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/

) {

const int32_t da = target.a - position.a,//position为上一个插补点的坐标，target-position为插补距离

db = target.b - position.b,

dc = target.c - position.c;

#if EXTRUDERS

int32_t de = target.e - position.e;

#else

constexpr int32_t de = 0;

#endif

uint8_t dm = 0;

#if CORE_IS_XY

......一大堆宏，看着好累

#else

if (da < 0) SBI(dm, X_AXIS);

if (db < 0) SBI(dm, Y_AXIS);

if (dc < 0) SBI(dm, Z_AXIS);

#endif

if (de < 0) SBI(dm, E_AXIS);

// Clear all flags, including the "busy" bit

block->flag = 0x00;

// Set direction bits //设置插补方向

block->direction_bits = dm;

.........

//设置各轴插补步数

block->steps.set(ABS(da), ABS(db), ABS(dc));

.........

//求出移动的距离s

block->millimeters = SQRT(

#if CORE_IS_XY

sq(delta_mm.head.x) + sq(delta_mm.head.y) + sq(delta_mm.z)

#elif CORE_IS_XZ

sq(delta_mm.head.x) + sq(delta_mm.y) + sq(delta_mm.head.z)

#elif CORE_IS_YZ

sq(delta_mm.x) + sq(delta_mm.head.y) + sq(delta_mm.head.z)

#else

sq(delta_mm.x) + sq(delta_mm.y) + sq(delta_mm.z)

#endif

);

//step_event_count设置为各轴最大移动步数

block->step_event_count = _MAX(block->steps.a, block->steps.b, block->steps.c, esteps);

.......

//求出距离倒数1/s

const float inverse_millimeters = 1.0f / block->millimeters; // Inverse millimeters to

remove multiple divides

float inverse_secs = fr_mm_s * inverse_millimeters;//求出时间的倒数1/t=v/s

......

//求出额定速度平方nominal_speed_sqr和额定速率nominal_rate

block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0

block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0

.......

//下面这段是设置加速度

// Start with print or travel acceleration

accel = CEIL((esteps ? settings.acceleration : settings.travel_acceleration) * steps_per_mm);

......

block->acceleration_steps_per_s2 = accel;

block->acceleration = accel / steps_per_mm;

........

//开始转角速度平方

float vmax_junction_sqr;

#if DISABLED(CLASSIC_JERK)

xyze_float_t unit_vec =

#if IS_KINEMATIC && DISABLED(CLASSIC_JERK)

delta_mm_cart

#else

{ delta_mm.x, delta_mm.y, delta_mm.z, delta_mm.e }

#endif

;

unit_vec *= inverse_millimeters;//求出当前线段单位向量 unit_vec={x/s,y/s,z/s}

......

// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.

if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {

// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)

// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.

//prev_unit_vec是上一段线段的单位向量，将unit_vec与-prev_unit_vec做点积就求出线段夹角余弦值cosθ

float junction_cos_theta = (-prev_unit_vec.x * unit_vec.x) + (-prev_unit_vec.y * unit_vec.y)

+ (-prev_unit_vec.z * unit_vec.z) + (-prev_unit_vec.e * unit_vec.e);

// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).

if (junction_cos_theta > 0.999999f) {

// For a 0 degree acute junction, just set minimum junction speed.

vmax_junction_sqr = sq(float(MINIMUM_PLANNER_SPEED));

}

else {

NOLESS(junction_cos_theta, -0.999999f); // Check for numerical round-off to avoid divide by zero.

// Convert delta vector to unit vector

xyze_float_t junction_unit_vec = unit_vec - prev_unit_vec;

normalize_junction_vector(junction_unit_vec);

const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec),

sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive.

//这里求sin(θ/2)

//应用转角公式计算最大转角速度 v^2=a*r

vmax_junction_sqr = (junction_acceleration * junction_deviation_mm * sin_theta_d2) / (1.0f - sin_theta_d2);

if (block->millimeters < 1) {

// Fast acos approximation, minus the error bar to be safe

const float junction_theta = (RADIANS(-40) * sq(junction_cos_theta) - RADIANS(50)) * junction_cos_theta + RADIANS(90) - 0.18f;

// If angle is greater than 135 degrees (octagon), find speed for approximate arc

if (junction_theta > RADIANS(135)) {

const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * junction_acceleration;

NOMORE(vmax_junction_sqr, limit_sqr);

}

}

}

// Get the lowest speed

vmax_junction_sqr = _MIN(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr);

}

else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.

vmax_junction_sqr = 0;

prev_unit_vec = unit_vec;

#endif

........

block->max_entry_speed_sqr = vmax_junction_sqr;//设置最大初速度为最大转角速度

// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.

const float v_allowable_sqr = max_allowable_speed_sqr(-block->acceleration, sq(float(MINIMUM_PLANNER_SPEED)), block->millimeters);//求出允许的最大速度,v_allowable_sqr^2 =2as+MINIMUM_PLANNER_SPEED^2

// If we are trying to add a split block, start with the

// max. allowed speed to avoid an interrupted first move.

block->entry_speed_sqr = !split_move ? sq(float(MINIMUM_PLANNER_SPEED)) : _MIN(vmax_junction_sqr, v_allowable_sqr);

.......

}

这里解释一下计算转角速度时的算法。如下图所示，P1P2与P2P3的夹角为θ，进而可求出求出sin(θ/2)=sqrt((1-cosθ)/2)，根据设置的弧长容差h，有：sin(θ/2)=r/(r+h)，进而可求出r=h*sin(θ/2)/(1-sin(θ/2))。有了r后可以由圆弧加速度公式：v*v=a*r，求出允许的最大转角速度v。

recalculate()函数

recalculate()函数代码：

void Planner::recalculate() {

// Initialize block index to the last block in the planner buffer.

const uint8_t block_index = prev_block_index(block_buffer_head);

// If there is just one block, no planning can be done. Avoid it!

if (block_index != block_buffer_planned) {

reverse_pass();

forward_pass();

}

recalculate_trapezoids();

}

速度前瞻算法在reverse_pass()和forward_pass()函数中实现，速度规划在recalculate_trapezoids()函数中实现。速度前瞻算法就是从当前待执行的区块往后规划很多个区块，使得每个区块的末速度等于前一个区块的初速度，并且每个区块的末速度与初速度满足关系:Ve^2-V0^2<=2*a*s。

reverse_pass()从当前新产生的区块往后递推到最前一个没有被处理过的区块，使得前后俩个区块的初速度V(i)满足V(i)^2<=V(i+1)^2+2*a*s。V(i+1)为该区块下一个区块的初速度。forward_pass()函数从最后一个被处理过的模块往前递推到当前新加入的区块，使得前后俩个区块的末速度满足V(i)^2<=V(i-1)^2+2*a*s。reverse_pass()规划的是区块的初速度，forward_pass()规划的是区块的末速度。

3 梯形速度与S形速度曲线规划

recalculate()中通过速度速度前瞻算法调整了各个区块以后，最后调用recalculate_trapezoids()执行速度曲线规划。该函数从当前已执行完的区块block_buffer_tail出开始往前到head_block_index，对之间的每个区块调用函数calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr)，执行速度曲线规划算法。速度曲线默认是梯形速度曲线，如果使能了S_CURVE_ACCELERATION，则执行S形曲线规划。

梯形速度规划

如左图所示，当采用梯形加速度法规划速度曲线时，初速度为