How to relate forward kinematics into PWM/speed?

Question

I am having a hard time understanding how to relate forward kinematics to things one can control in a robot like PWM and reading encoder values.

For example, how to relate the encoder values from motors and RPM to PWM values of the motors to make a robot follow a curved path?

Answer 1

Forward kinematics is used to compute the pose (position and orientation) the end-effector of a robot from the joint angles. Simialry, higher order forward kineamtics can be defined to calcualte the velocities of the end-effector from the volocities (and angular positions) of the joints. The same can be done for accelerations.

Inverse kinematics represents the same transformation but in a reversed direction. It is used to calculate the joint angles from the end-effectro pose. Simialry, higher order inverse kinematics can be defined for calculating joint velocities from end-effectro velocities (and pose). The same can be done for accelerations.

A curve is given in Cartesian space as a string of points. For each point the inverse kinematics problem can be solved, and so, a string of points in joint space can be computed. This string of points will consist of joint angle coordinates. If the joints are set to thee values, one after another, the end-effectro will describe the given path. Please be advised, that the points have to be close to one another. The closer they are, the more precise the curve will be.

The motors of all joints have to be commanded to turn in the positive or negative direction in order to reach the desired point. This is what the PID controller does. It gets the reference value (the desired angle) and then calculates the PWM duty cycle toe make the motor move. The PWM duty cycle, as a simplistic explanation, is proportional to the maximum speed of the motor. 100% duty cycle will turin the motor with full speed, 50% will turin the motor with half speed. The PID Controller calcultes the PWM duty cycle based on how far the desired point is from the current point. If far, high speeds will be reached, if it is near, only slow speed will be necesary.

So, if you want to make the robot follow a path, a simplisic approach is to:

1. Define the curve in cartesian space a string of points
2. Compute the joint coordinates of the robot for each point
3. Select the First point in the list
4. Command the motors of the robot to turn to the selected point
5. Wait for the robot to arrive
6. If there are point left in the list select the next point in the list and go to 3 if not end

A more advancced appraoch would need also velocity information from the Cartesian Space and would transfomr those velocitey to joint space and not just set motor positions but also motor velocities to generate a smoother motion.

Answer 2

Usually you do this by implementing a feedback-loop.

Theory

By generating a PWM Signal you are (over time average) generating a voltage at your motor. As a approximitation the voltage is proportional to the speed of the motor (this could be redefined by measuring the relationship between voltage and speed).

This could be decribed by:

where r is the rotation as a function of the duty-cycle p.

Now you know which rotation your motor should have. Now you can implement a feedback loop, you know how fast your motor is running and how fast your motor is actually running so you can define delta r:

This delta r can be used to implement a PID Controller so your motor always rotates at the speed it should. The power P of your PID controller is defined by (the three constant P,I,D are device dependent):

With a good setup your motor rotates now more or less always at the exact speed it should. The distance s your robots travells over time is now given by:

with p(t) beeing the power as a function of time.

Implementation

In this example motorPower(int16_t power) is a function which controls the motor with a power between -255 and 255. This function does not reads the current motor speed. The function currMotorSpeed() returns the current speed of the motor, for simplicity reasons also a value between -255 and 255. The constant dt is the time between each execution of the PID controller.

Implementation in C++:

// PID Controll for the motor
int16_t motorPowerControlled(int16_t motorPower){
    static int iArea, dLastValue;
    int16_t deltaPower = currMotorSpeed() - motorPower;

    int16_t pValue = deltaPower * P_CONSTANT

    iArea += deltaPower;
    int16_t iValue = iArea * dT * I_CONSTANT

    int16_t dValue = (deltaPower-dLastValue)/dt * D_FACTOR;
    dLastValue = deltaPower;

    return pValue + iValue + dValue;
}


int main(void){
    int16_t distanceDriven = 0;
    const int16_t POWER = 100;

    while(true){
        motorPowerController(POWER);
        distanceDriven += POWER * dt;
        delay_ms(dt);
    }
}