问:
Quaternion error in one degree of freedom
I would like to control a system which orientation I measure with as quaternion based EKF in one degree of freedom. Obviously the simplest thing would be to convert the quaternion estimate to euler angles and take one of these. Unfortunately these euler angles are not stable. The other option I was thinking about was to use the so called quaternion error, where I specify another quaternion which I want my system to be controlled towards. Then I would have to specify all four values of this quaternion. </p>

<p>I'm wondering if there is a way to generate a quaternion error only for one of my angles? </p>

答:
You will have to specify the full quaternion in any case and that is because each w,x,y and z are a function of all the Euler angles.

Some ways to get by could be:

If you are concerned about an EKF generating noisy output in the other two angles: in the update step, you can hard code these known angles

If you want to control just one angle: just extract one single value of this rotation (Euler angle would be your best bet) or extract the entire quaternion from this EKF and find the error with the desired angle or the quaternion.

Your desired quaternion should not be difficult to calculate since you either want the robot to be same in the other two angles or you know the desired angles already.

Note : if you deal with Euler angles, you will have to be careful of the gimbal lock problem (singularity) which should not be very difficult to overcome.


问:
What is the advantage having seven degrees of freedom in a robotic arm?
I understand what six degrees of freedom is, but I encountered the term of seven degrees of freedom at multiple occasions in robotics. What does is the purpose of that extra degree of freedom?</p>

答:
I guess you are talking about robot arms with 7 DoF like the LBR IIwa from Kuka.

If you have 6 DoF, a given end-effector pose is only reachable in a single configuration of the robot. If you have additional joints, you can still move the arm without moving the end-effector so that for example the ellbow of the arm is not colliding with parts of the environment.

A common scenario would be to place an object into a CNC machine. Your end-effector pose is fixed, but the additional dof gives you more freedom so that your arm does not hit the doors of the machine.

ContentLicense: CC BY-SA 4.0


问:
Are Indirekt KFs with non-linear changing statematrix EKFs for the error state?
I'm trying to implement an indirect kalman filter to estimate the pose of a differential drive robot using gyroscope and wheel encoder data.

I found a fiew papers (1 - 3) describing this approach but am confused about one thing. The state-matrix looks something like this:

Elements of this matrix change over time in a non-linear fashon which seems wrong to me:

The state matrix should be static and changing values should be encapsuled in the controll input.

Non-linear changes should only occur when using an EKF or UKF eventhough all papers only mention Kalman Filter

Are these two points just left out or simplified for conveniece since its obvious or am I missunderstanding something?

Dead Reckoning Navigation for Autonomous Mobile Robots. IFAC Proceedings Volumes. 1. March 1998;31(3):219–24.

Panich. Indirect Kalman Filter in Mobile Robot Application. Journal of Mathematics and Statistics. 1. August 2010;6(3):381–4.

Zunaidi I, Kato N, Nomura Y, Matsui H. Positioning System for 4-Wheel Mobile Robot: Encoder, Gyro and Accelerometer Data Fusion with Error Model Method. 2006;

答:
I am pretty sure that what you want is an extended kalman filter since the dynamics of the robot have to be non-linear based off the state matrix you have shown.

```<code>The state matrix should be static and changing values should be encapsuled in the controll input.
Non-linear changes should only occur when using an EKF or UKF eventhough all papers only mention Kalman Filter
</code>```

You are correct on these statements. Although I could not go through 1st and 3rd paper. 2nd paper didn't have the state matrix you have pasted here.

Body:
I am pretty sure that what you want is an extended kalman filter since the dynamics of the robot have to be non-linear based off the state matrix you have shown.

The state matrix should be static and changing values should be encapsuled in the controll input.
Non-linear changes should only occur when using an EKF or UKF eventhough all papers only mention Kalman Filter

You are correct on these statements. Although I could not go through 1st and 3rd paper. 2nd paper didn't have the state matrix you have pasted here.


问:
Implementing ESKF
I'm currently struggling with implementing the Multiplicative Kalman Filter or Error State Kalman Filter as described by Landis Markley in Attitude Error Representations for Kalman Filtering. Sadly there is two versions of almost the same paper online:

at nasa.gov

at ResearchGate

First of all, I'm a little bit confused why eq. 45 in 1 is different from eq. 33 in 2. Which one is correct? Usually the covariance propagation is defined as $P = FPF^T + GQG^T$. What is the reason for using $P = FP + PF^T + GQG^T$?

In the paper Markley just filters the attitude error and the gyro drift. I would also like to have the angular velocity and acceleration as part of the state. If I understand it right, Markley integrates gyro measurements in the predict step. At the moment I'm trying to figure out the correct way of having the angular velocity as part of the state and then performing a separate update step for the gyroscope.

Currently I have the following code:

import numpy as np

class ESKF:
    """This is an Error State Kalman Filter (ESKF) as described by Markley
    see https://www.researchgate.net/publication/245432681_Attitude_Error_Representations_for_Kalman_Filtering

    """

    def __init__(self):
        # 3 x attitude error, 3 x gyro bias, 3 x angular velocity
        self.x = np.zeros(9)
        # reference quaternion
        self.qref = np.array([0.0, 0.0, 0.0, 1.0])
        # state covariance
        self.P = np.identity(9)
        # process noise
        self.Q = np.identity(9) * 0.01 # TODO should be determined by tests
        # sensor noise
        self.R_gyr = np.identity(3) * 0.01 # TODO should be determined for sensor
        self.R_acc = np.identity(3) * 0.01 # TODO should be determined for sensor
        self.R_mag = np.identity(3) * 0.01 # TODO should be determined for sensor

    #TODO initialization

    def predict(self, dt):
        """The ESKF predict step

        Parameters
        ----------
        dt : float
             The time step in s

        """
        # eq. 23
        self.qref += 0.5 * quat_mult(np.array([self.x[6], self.x[7], self.x[8], 0]), self.qref) * dt
        # normalize to compensate numerical errors
        self.qref /= np.linalg.norm(self.qref)

        # eq. 38
        F = np.zeros((9,9))
        # df/da
        F[0:3,0:3] = - vec_to_cross_matrix(self.x[6:9])
        # df/db
        F[0:3,3:6] = - np.identity(3)
        # df/dw
        F[0:3,6:9] = vec_to_cross_matrix(self.x[0:3])

        #eq. 39
        G = np.zeros((9,9))
        G[0:3,0:3] = -np.identity(3)
        G[3:6,3:6] = np.identity(3)
        G[6:9,6:9] = np.identity(3)

        # eq. 33
        self.P = F @ self.P + self.P @ F.T + G @ self.Q @ G.T
        #self.P = F @ self.P @ F.T + G @ self.Q @ G.T

    def update_gyro(self, z):
        """The ESKF update step for a gyrosope

        Parameters
        ----------
        z : array, shape [3]
            Sensor measurement with structure [x, y, z]

        """
        # Kalman Gain
        # K = np.zeros((3,3))
        H = np.zeros((3,6))
        # expected measurement is angular velocity + gyro drift
        H[0:3,0:3] = np.identity(3)
        H[0:3,3:6] = np.identity(3)
        # K = P * H' (H * P * H' + R)^-
        K = self.P[3:9,3:9] @ H.T @ np.linalg.inv(H @ self.P[3:9,3:9] @ H.T + self.R_gyr)
        # x = x + K * (z - H * x)
        self.x[3:9] += K @ (z - H @ self.x[3:9])

        # P = (I - KH)P
        IKH = np.identity(6) - K @ H
        self.P[3:9,3:9] = IKH @ self.P[3:9,3:9]

    def update_acc(self, z):
        """The ESKF update step for an accelerometer

        Parameters
        ----------
        z : array, shape [3]
            Sensor measurement with structure [x, y, z]

        """
        vi = np.array([0, 0, -9.81]) # TODO + acc
        # eq. 42
        vb_pred = rotvec_to_mat(self.x[0:3]) @ quat_to_mat(self.qref) @ vi

        # h(v) = v
        h = vb_pred
        # Ha
        # eq. 44
        Ha = vec_to_cross_matrix(vb_pred)
        #eq. 46
        K = self.P[0:6,0:3] @ Ha.T @ np.linalg.inv(Ha @ self.P[0:3,0:3] @ Ha.T + self.R_acc)

        # eq. 47
        self.x[0:6] += K @ (z - h - Ha @ self.x[0:3])
        # eq. 48
        self.P[0:6,0:6] -= K @ Ha @ self.P[0:3,0:6]

    def update_mag(self, z, B, incl, W, V):
        """The ESKF update step for a magnetometer
        see https://www.nxp.com/docs/en/application-note/AN4246.pdf

        Parameters
        ----------
        z : array, shape [3]
            Sensor measurement with structure [x, y, z]
        B : float
            The geomagnetic field strength in gauss
        incl : float
               The inclination angle in rad
        W : array, shape [3,3]
            The soft-iron distortion
        V : array, shape [3]
            The hard-iron distortion

        """
        vi = B * np.array([np.cos(incl), 0, -np.sin(incl)])
        # eq. 42
        vb_pred = rotvec_to_mat(self.x[0:3]) @ quat_to_mat(self.qref) @ vi

        #h(v) = W * v + V
        h = W @ vb_pred + V
        # Ha
        # eq. 44
        Ha = W @ vec_to_cross_matrix(vb_pred)
        #eq. 46
        K = self.P[0:6,0:3] @ Ha.T @ np.linalg.inv(Ha @ self.P[0:3,0:3] @ Ha.T + self.R_mag)

        # eq. 47
        self.x[0:6] += K @ (z - h - Ha @ self.x[0:3])
        # eq. 48
        self.P[0:6,0:6] -= K @ Ha @ self.P[0:3,0:6]

    def reset(self):
        """The ESKF reset step

        """
        # eq. 14
        self.qref = quat_mult(gibbs_vec_to_quat(self.x[0:3]), self.qref)
        self.x[0:3] = np.zeros(3)
```

<p>and some helpers defined the following:</p>

<pre class="lang-py prettyprint-override"><code>def quat_mult(a, b):
    """Multiply 2 quaternions. They should have the structure [v1, v2, v3, w]

    Parameters
    ----------
    a : array, shape [4]
        Quaternion 1
    b : array, shape [4]
        Quaternion 2

    Returns
    -------
    q : array, shape [4]
        Quaternion product of a and b

    """
    # eq. 6
    v = a[3] * b[0:3] + b[3] * a[0:3] - np.cross(a[0:3], b[0:3])
    w = a[3] * b[3] - a[0:3].T @ b[0:3]
    return np.array([ v[0], v[1], v[2], w ])

def vec_to_cross_matrix(a):
    """Constructs the skew symmetric cross product matrix of a

    Parameters
    ----------
    a : array, shape [3]
        Vector

    Returns
    -------
    M : array, shape [3,3]
        Cross product matrix of a

    """
    # eq. 5
    return np.array([[0, -a[2], a[1]], [a[2], 0, -a[0]], [-a[1], a[0], 0]])

def quat_to_mat(a):
    """Converts a quaternion into a rotation matrix

    Parameters
    ----------
    a : array, shape [4]
        Quaternion

    Returns
    -------
    M : array, shape [3,3]
        The rotation matrix of a

    """
    # eq. 4
    return (a[3]**2 - a[0]**2 - a[1]**2 - a[2]**2) * np.identity(3) - 2 * a[3] * vec_to_cross_matrix(a[0:3]) + 2 * a[0:3] @ a[0:3].T

def rotvec_to_mat(a):
    """Converts a rotation vector into a rotation matrix

    Parameters
    ----------
    a : array, shape [3]
        Rotation vector

    Returns
    -------
    M : array, shape [3,3]
        The rotation matrix of a

    """
    # eq. 20
    a_norm_sqr = a[0]**2 + a[1]**2 + a[2]**2
    return np.identity(3) - vec_to_cross_matrix(a) - 0.5 * (a_norm_sqr * np.identity(3) - a @ a.T)

def gibbs_vec_to_quat(a):
    """Converts a gibbs vector into a quaternion

    Parameters
    ----------
    a : array, shape [3]
        Gibbs vector

    Returns
    -------
    q : array, shape [4]
        The quaternion of a with structure [v1, v2, v3, w]

    """
    # eq. 18b
    a_norm_sqr = a[0]**2 + a[1]**2 + a[2]**2
    return (1 / np.sqrt(4 + a_norm_sqr)) * np.concatenate((a, [2]))
</code>```

Obviously, the angular velocity and the gyro drift always have the same values. Is it worth to keep both in the state, or should I just abandon the gyro drift?

I also miss one step in the derivation of eq. 43 in 2. Let $A(a) = \{ I_{3 \times 3} - [\mathbf{a} \times] - \frac{1}{2} ( a^2 I_{3 \times 3} - \mathbf{a}\mathbf{a}^T ) \}$. Then the Taylor expansion is $h(v_B) = h(\bar{v}_B) + \frac{\delta h}{\delta v} |_{\bar{v}_B} (A(a) - I_{3 \times 3}) \bar{v}_B$ since $v_B = A(a)A(q_{ref})v_I = A(a)\bar{v}_B$. But how is this collapsed too just $[\mathbf{a} \times] \bar{v}_B$?

When I let $v_I = (\begin{smallmatrix} 0 & 0 & -9.81 \end{smallmatrix})^T + (\begin{smallmatrix} accx & accy & accz \end{smallmatrix})^T$ and $acc$ is part of my state, then I also have to include $\frac{\delta h}{\delta acc}$ to $H$ (eq. 45 in 2), right? Taking this derivative looks fairly complicated. Is there a smarter way doing it? EDIT: It's not, it's just $\frac{\delta h}{\delta v} A(a) A(q_{ref})$

Thanks in advance,
Martin

答:
@fibonatic was absolutely correct in the comments about the NASA paper talking about the continuous version of it.

By just one look of the equation, I could tell since all of the variables were a function of time and it is a differential equation.

Title: Implementing ESKF


答:
Because the pose of an end effector is 6 dimensional (position(x, y,z) and orientation(ex, ey, ez)), a 6 DOF manipulator is atleast needed to achieve a desired pose. Theoretically, there can be infinite configurations of the robot to achieve the 6D pose.

However, lots of those configurations could have singularities or self/world collisions. There could joint position and torque limits as well, that can curtail the robot's ability to have a feasible inverse kinematics solution and achieve the pose.

Additional degrees of freedom, 7 or more are usually used to overcome the limitations of a 6DOF robot. With 7 or more DOFs, the robot can handle and overcome singularities because there is a larger probability of having atleast 6 DOFs to achieve the desired pose.

Null space optimization is also used here such that the system/robot has redundancy and can use the additional degrees of freedom to pursue secondary tasks like avoiding collisions, maintaining low energy and maintaining a particular elbow/shoulder/wrist configuration.

Title: What is the advantage having seven degrees of freedom in a robotic arm?

Body: I understand what six degrees of freedom is, but I encountered the term of seven degrees of freedom at multiple occasions in robotics. What does is the purpose of that extra degree of freedom?</p>


问:
Detecting the presence of a person in a room
I am working on my first hobby project and I'm not very familiar with sensors yet. I am trying to build a system which detects the presence of a person in a small room with a single entrance/exit door.

The idea is when the first person enters the room, the lights turn on and any following person doesn't affect state of the lights. After the last person leaves, the lights should turn off. In a programmatic sense, the lights should turn on when present person count is greater than 0.

I have explored my options and found out that infrared sensors are usually used for this type of problem. What I am not sure is how to detect whether person has entered or left, so I would like to ask for some help with this.

答:
You can use two infrared sensors one at the outside and one inside of the room. You can detect by using algorithm such as sensor outside= A sensor inside=B
If AB then +1 person if BA then -1 person
Use a counter program to determine people in room.</p>

ContentLicense: CC BY-SA 3.0


问:
Calculation of error, based on camera images
I am trying to measure the precision error of a gripper that is mounted to the flange of a 6 DOF industrial robot. The gripper-design is rather complex, therefore we would like to make sure it is operating precisely.

I think a simple way to measure the gripper-precision is to use a camera and visual markers on a piece that is gripped. See the following illustration of the setup. The colorful box mocks a part that is being gripped by the gray gripper that is mounted to the robot flange.

To get data to work with, I imagine the following procedure:

the robot picks up part 1

the robot holds the red side of the part in front of the camera, which takes a picture

the robot rotates the part by actuating its 6th joint by 90 degrees, the camera takes a picture of the green side

the robot rotates the part by another 90 degrees, the camera takes a picture (back side)

the robot rotates the part by another 90 degrees, the camera takes a picture

the robot places the part back in the nest

go back to step 1, until 10 iterations are completed

In summary: Each time the robot gripped the part, a batch of pictures from different sides are taken.

The error, that needs to be calculated, is defined as the pose-difference of the part that is caused by small errors in the gripper mechanism. The error has 6 dimensions.

The camera can be assumed to be calibrated.

Using two batches of pictures from two different grips: How can I use the image plane coordinates of the $N$ features attached to a part, to calculate the pose-difference of the part?

答:
Rather than rotating the box, I would attach a calibration chessboard on the box to calculate the extrinsic. If you know the homography matrix between your camera and the board you can calculate the extrinsic(have a look at zhang camera calibration method).

Or using multiple cameras is one good way around. Place two cameras so that they can see each side of the box and calibrate their extrinsic within the range your robot holds the box.

I guess rotating 6th axis might not be a good idea as you need a calibration between the robot and camera.


问:
Output power of an actuator - General definition
I'm looking for a general definition of the output power of an actuator. Can you suggest me any book or paper or education website where I can find it please?

Thank you in advance.

EDIT

I read the paper "Youqi Wang 2004 - Deformation mechanisms of electrostrictive graft elastomer" where I found the locution "output power". I'd like to know a general definition of the output power of an actuator.

答:
It reads to me like the author is stating that a stiffer material means it could make an actuator with more output power, which doesn't make a lot of sense to me.

Maybe they mean a more efficient actuator? Actuator output power is just any definition of mechanical power; either a force times the linear speed or a torque times the rotational speed.

A stiffer material may mean that the input power is transmitted more efficiently to the output end of the actuator because the motion isn't "consumed" by flex or deformation in the actuator material.

More efficient actuators then would imply that you could make an actuator that had the same weight but a higher output power, but that's specific power, or the power-to-weight ratio. This may be what the author meant - stiffer materials could lead to actuators with greater specific power output.

You should try to contact the author(s) for more information! I've only tried contacting authors a couple times, but I've gotten really friendly responses every time I have.


问:
3-RRP manipulator robot
I'm having some issues with a planar manipulator I have to deal with.

The first two moves are rotational and the third one is a prismatic one. The shoulder and elbow move around the X-axis, while the forearm moves forward and backward a distance "a" (d3).

I did all the direct and inverse kinematics, including the Jacobian. I think the third move around x is not done correctly.

Here is a picture about the x-y-z-axis and the DH Table we made:

I've seen an answer about this subject but I can't even understand that one.
On the image you can see there are my two attempts, but neither of them worked for me.

答:
There are 2 main rules in assigning frames following DH convention: source: Robot Modeling and Control, Spong et. al

$x_i \perp z_{i-1}$

$x_i$ intersects $z_{i-1}$

In your attempts,

Your first attempt is incorrect as your $z_2$ axis is not in  the direction of actuation of the prismatic joint.

I'm not clear on your second attempt as $x_1$ appears to be named at 2 places. (Please correct your image).

One possible frame assigment I'm assuming is with the following correction to your second attempt image:

$x_1$ is in the direction of $z_2$ and $x_2 \perp z_2$ as in your second figure.

The only correction you need to make in your DH table is in second row, i.e. change $\theta_2$ to $\theta_2-\pi/2$ as you are rotating $x_1$ to $x_2$ around $z_1$ by $-\pi/2$.

Hope this helps your situation.

-Cheers!

Title: 3-RRP manipulator robot


问:
Can the rate of peristaltic pump's flow be accurate across changes in fluid viscosity?
I'm building an arduino controlled pump system to be able to move fluids around. I need this to be fairly accurate, but extreme precision isn't required. Since there will be a variety of liquids moved through the pump, I've determined a peristaltic pump the best fit. But I don't think I fully understand them, and had a few questions..

Since I'll need to purge the system... Can a peristaltic pump push air? Let's assume you have a 2m of tubing, and you pump a bunch of water through it. Can you remove the tube from the water reservoir so it is open to the air, and effectively purge the system of any remaining water?

Since I want to fairly accurately measure flow, could I simply count milliseconds instead of using a flowmeter? ... Will a peristaltic pump ALWAYS pump at a constant rate, regardless of the viscosity of the fluid? That is, will maple syrup come out at the same rate as water?

Shopping question, ignore I suppose ... Anyone know where I may find a fast/high flow peristaltic pump? I'm looking to be able to pump, at a minimum, .5oz/sec

Would be determinant upon #3 ... What sort of relay would I want for toggling this on/off with an arduino?

答:
I will try to answer 2 and 3.

2

I am pretty sure the cheap ones won't and I doubt expensive ones would.

In addition, The viscosity isn't the only thing you have to take into account when it comes to accuracy. Distance, height, wear of the tubes, wear of the motor, liquids that can potentially accumulate in the tube, etc, etc.

Having that said, I wouldn't design the solution expecting constant rate. I would rather provide an easy interface to calibrate the pumps to a particular situation. Also, I would recommend the user to check calibration in a regular basis (something like 6 months, by experience).

3

It doesn't really matter, you can use a simple relay, a solid state relay or a mosfet.
A professional level custom circuit would use a mosfets, as it is silent, small and fits better in a PCB.
Personally, I would rather use a relay board for DIY stuff, which is surprisingly cheaper and easier to find in single units these days. Also, it requires no soldering and doesn't require you to learn how to use the mosfet.


答:
After some work I got it working. You can find the C++ source code on github. Don't use the python code in the question. It has some flaws.

In the end I just abandoned the gyro drift (I would argue that this is unobservable with just one gyro). Furthermore I used the classic covariance propagation $P_{k+1} = FP_kF^T + GQG^T$.

Title: Implementing ESKF

Body:
I'm currently struggling with implementing the Multiplicative Kalman Filter or Error State Kalman Filter as described by Landis Markley in Attitude Error Representations for Kalman Filtering. Sadly there is two versions of almost the same paper online:

at nasa.gov

at ResearchGate

First of all, I'm a little bit confused why eq. 45 in 1 is different from eq. 33 in 2. Which one is correct? Usually the covariance propagation is defined as $P = FPF^T + GQG^T$. What is the reason for using $P = FP + PF^T + GQG^T$?

In the paper Markley just filters the attitude error and the gyro drift. I would also like to have the angular velocity and acceleration as part of the state. If I understand it right, Markley integrates gyro measurements in the predict step. At the moment I'm trying to figure out the correct way of having the angular velocity as part of the state and then performing a separate update step for the gyroscope.

Currently I have the following code:

import numpy as np

class ESKF:
    """This is an Error State Kalman Filter (ESKF) as described by Markley
    see https://www.researchgate.net/publication/245432681_Attitude_Error_Representations_for_Kalman_Filtering

    """

    def __init__(self):
        # 3 x attitude error, 3 x gyro bias, 3 x angular velocity
        self.x = np.zeros(9)
        # reference quaternion
        self.qref = np.array([0.0, 0.0, 0.0, 1.0])
        # state covariance
        self.P = np.identity(9)
        # process noise
        self.Q = np.identity(9) * 0.01 # TODO should be determined by tests
        # sensor noise
        self.R_gyr = np.identity(3) * 0.01 # TODO should be determined for sensor
        self.R_acc = np.identity(3) * 0.01 # TODO should be determined for sensor
        self.R_mag = np.identity(3) * 0.01 # TODO should be determined for sensor

    #TODO initialization

    def predict(self, dt):
        """The ESKF predict step

        Parameters
        ----------
        dt : float
             The time step in s

        """
        # eq. 23
        self.qref += 0.5 * quat_mult(np.array([self.x[6], self.x[7], self.x[8], 0]), self.qref) * dt
        # normalize to compensate numerical errors
        self.qref /= np.linalg.norm(self.qref)

        # eq. 38
        F = np.zeros((9,9))
        # df/da
        F[0:3,0:3] = - vec_to_cross_matrix(self.x[6:9])
        # df/db
        F[0:3,3:6] = - np.identity(3)
        # df/dw
        F[0:3,6:9] = vec_to_cross_matrix(self.x[0:3])

        #eq. 39
        G = np.zeros((9,9))
        G[0:3,0:3] = -np.identity(3)
        G[3:6,3:6] = np.identity(3)
        G[6:9,6:9] = np.identity(3)

        # eq. 33
        self.P = F @ self.P + self.P @ F.T + G @ self.Q @ G.T
        #self.P = F @ self.P @ F.T + G @ self.Q @ G.T

    def update_gyro(self, z):
        """The ESKF update step for a gyrosope

        Parameters
        ----------
        z : array, shape [3]
            Sensor measurement with structure [x, y, z]

        """
        # Kalman Gain
        # K = np.zeros((3,3))
        H = np.zeros((3,6))
        # expected measurement is angular velocity + gyro drift
        H[0:3,0:3] = np.identity(3)
        H[0:3,3:6] = np.identity(3)
        # K = P * H' (H * P * H' + R)^-
        K = self.P[3:9,3:9] @ H.T @ np.linalg.inv(H @ self.P[3:9,3:9] @ H.T + self.R_gyr)
        # x = x + K * (z - H * x)
        self.x[3:9] += K @ (z - H @ self.x[3:9])

        # P = (I - KH)P
        IKH = np.identity(6) - K @ H
        self.P[3:9,3:9] = IKH @ self.P[3:9,3:9]

    def update_acc(self, z):
        """The ESKF update step for an accelerometer

        Parameters
        ----------
        z : array, shape [3]
            Sensor measurement with structure [x, y, z]

        """
        vi = np.array([0, 0, -9.81]) # TODO + acc
        # eq. 42
        vb_pred = rotvec_to_mat(self.x[0:3]) @ quat_to_mat(self.qref) @ vi

        # h(v) = v
        h = vb_pred
        # Ha
        # eq. 44
        Ha = vec_to_cross_matrix(vb_pred)
        #eq. 46
        K = self.P[0:6,0:3] @ Ha.T @ np.linalg.inv(Ha @ self.P[0:3,0:3] @ Ha.T + self.R_acc)

        # eq. 47
        self.x[0:6] += K @ (z - h - Ha @ self.x[0:3])
        # eq. 48
        self.P[0:6,0:6] -= K @ Ha @ self.P[0:3,0:6]

    def update_mag(self, z, B, incl, W, V):
        """The ESKF update step for a magnetometer
        see https://www.nxp.com/docs/en/application-note/AN4246.pdf

        Parameters
        ----------
        z : array, shape [3]
            Sensor measurement with structure [x, y, z]
        B : float
            The geomagnetic field strength in gauss
        incl : float
               The inclination angle in rad
        W : array, shape [3,3]
            The soft-iron distortion
        V : array, shape [3]
            The hard-iron distortion

        """
        vi = B * np.array([np.cos(incl), 0, -np.sin(incl)])
        # eq. 42
        vb_pred = rotvec_to_mat(self.x[0:3]) @ quat_to_mat(self.qref) @ vi

        #h(v) = W * v + V
        h = W @ vb_pred + V
        # Ha
        # eq. 44
        Ha = W @ vec_to_cross_matrix(vb_pred)
        #eq. 46
        K = self.P[0:6,0:3] @ Ha.T @ np.linalg.inv(Ha @ self.P[0:3,0:3] @ Ha.T + self.R_mag)

        # eq. 47
        self.x[0:6] += K @ (z - h - Ha @ self.x[0:3])
        # eq. 48
        self.P[0:6,0:6] -= K @ Ha @ self.P[0:3,0:6]

    def reset(self):
        """The ESKF reset step

        """
        # eq. 14
        self.qref = quat_mult(gibbs_vec_to_quat(self.x[0:3]), self.qref)
        self.x[0:3] = np.zeros(3)
```

<p>and some helpers defined the following:</p>

<pre class="lang-py prettyprint-override"><code>def quat_mult(a, b):
    """Multiply 2 quaternions. They should have the structure [v1, v2, v3, w]

    Parameters
    ----------
    a : array, shape [4]
        Quaternion 1
    b : array, shape [4]
        Quaternion 2

    Returns
    -------
    q : array, shape [4]
        Quaternion product of a and b

    """
    # eq. 6
    v = a[3] * b[0:3] + b[3] * a[0:3] - np.cross(a[0:3], b[0:3])
    w = a[3] * b[3] - a[0:3].T @ b[0:3]
    return np.array([ v[0], v[1], v[2], w ])

def vec_to_cross_matrix(a):
    """Constructs the skew symmetric cross product matrix of a

    Parameters
    ----------
    a : array, shape [3]
        Vector

    Returns
    -------
    M : array, shape [3,3]
        Cross product matrix of a

    """
    # eq. 5
    return np.array([[0, -a[2], a[1]], [a[2], 0, -a[0]], [-a[1], a[0], 0]])

def quat_to_mat(a):
    """Converts a quaternion into a rotation matrix

    Parameters
    ----------
    a : array, shape [4]
        Quaternion

    Returns
    -------
    M : array, shape [3,3]
        The rotation matrix of a

    """
    # eq. 4
    return (a[3]**2 - a[0]**2 - a[1]**2 - a[2]**2) * np.identity(3) - 2 * a[3] * vec_to_cross_matrix(a[0:3]) + 2 * a[0:3] @ a[0:3].T

def rotvec_to_mat(a):
    """Converts a rotation vector into a rotation matrix

    Parameters
    ----------
    a : array, shape [3]
        Rotation vector

    Returns
    -------
    M : array, shape [3,3]
        The rotation matrix of a

    """
    # eq. 20
    a_norm_sqr = a[0]**2 + a[1]**2 + a[2]**2
    return np.identity(3) - vec_to_cross_matrix(a) - 0.5 * (a_norm_sqr * np.identity(3) - a @ a.T)

def gibbs_vec_to_quat(a):
    """Converts a gibbs vector into a quaternion

    Parameters
    ----------
    a : array, shape [3]
        Gibbs vector

    Returns
    -------
    q : array, shape [4]
        The quaternion of a with structure [v1, v2, v3, w]

    """
    # eq. 18b
    a_norm_sqr = a[0]**2 + a[1]**2 + a[2]**2
    return (1 / np.sqrt(4 + a_norm_sqr)) * np.concatenate((a, [2]))
</code>```

Obviously, the angular velocity and the gyro drift always have the same values. Is it worth to keep both in the state, or should I just abandon the gyro drift?

I also miss one step in the derivation of eq. 43 in 2. Let $A(a) = \{ I_{3 \times 3} - [\mathbf{a} \times] - \frac{1}{2} ( a^2 I_{3 \times 3} - \mathbf{a}\mathbf{a}^T ) \}$. Then the Taylor expansion is $h(v_B) = h(\bar{v}_B) + \frac{\delta h}{\delta v} |_{\bar{v}_B} (A(a) - I_{3 \times 3}) \bar{v}_B$ since $v_B = A(a)A(q_{ref})v_I = A(a)\bar{v}_B$. But how is this collapsed too just $[\mathbf{a} \times] \bar{v}_B$?

When I let $v_I = (\begin{smallmatrix} 0 & 0 & -9.81 \end{smallmatrix})^T + (\begin{smallmatrix} accx & accy & accz \end{smallmatrix})^T$ and $acc$ is part of my state, then I also have to include $\frac{\delta h}{\delta acc}$ to $H$ (eq. 45 in 2), right? Taking this derivative looks fairly complicated. Is there a smarter way doing it? EDIT: It's not, it's just $\frac{\delta h}{\delta v} A(a) A(q_{ref})$

Thanks in advance,
Martin


问:
Robotic Manipulator 3-RRP v2
I recently asked a question about a 3-RRP manipulator but the only answer didn't work for my robot.

I would describe the manipulator as an arm that rotates around the shoulder over the x-y axis, the elbow  that rotates around the x-y axis also and the forearm - hand that moves FORWARD and BACKWARDS. (check attached image for a more described explanation).

The solution I calculated cannot be correct as the "hand" moves up and down and not forward and backward.

I tried setting other values to the DH-parameters but I still think this is not the correct answer.

My professor said that we are not allowed to use a structural parameter to rotates the axes and that the use of 3 rows on the DH-parameters is the correct solution.

I've been trying to get this for almost 1 week now and seems impossible to me.

On the image you can see some of my attempts to solve this problem.

A detailed answer with the reason for the values of the DH-parameters would be awesome.

答:
I'm unable to comment due to reps shortage, so, I'm adding it as an answer. This might answer your question.

In your new attempt, the transformation of axes from origin-1 (i.e. $x_1, y_1, z_1$) to origin-2 ($x_2, y_2, z_2$) is incorrect as $x_2 \not\perp z_1$ and $x_2 \not\cap z_1$, violating DH convention.

PS: It would be better if you could update your previous question itself with the new attempt you made so that everything is in one place.

Title: Robotic Manipulator 3-RRP v2


问:
motor controlled lazy susan
I'm trying to figure out an easy lazy susan setup.  This is for video production, I want to be able to put an object on the platter and get a smooth consistent turn in order to shoot the object spinning.  What would be the best way/parts to put this together?

Thanks

答:
You can design, or purchase, a simple rotary table.  I have used Arrick robotics over the years, and they have a relatively turnkey system for $300 here:  https://www.arrickrobotics.com/rt12.html

If you are skilled at automation you could do this with a dc brushed motor and a platter, but it sounds from the way you ask your question that a turnkey system might better solve your problem.

Title: motor controlled lazy susan


问:
Is ROS still maintained are any other trendy alternatives?
I know this question is off-topic, but here I might get the answer.

Recently I started collaborating in a project based on ROS and I find several examples outdated, some repositories I am interested in have hundred of pull requests but are not merged. The supported Ubuntu versions are old.

My point is, ROS project does not give me confidence as other open-source projects. I hope I am wrong, but that is my impression after going throwout the guides and other documents.

By the way, the guides for beginners are quite detailed and easy to follow, but when one starts digging deeper, finds the lack of activity, like if people migrated, using other alternatives.

Either, the project is not widely adopted, or consumers of this software make their own improvements and keep them private.

If this is the reality, are there other know alternatives?

答:
ROS Melodic supports 18.04, the current LTS-version of Ubuntu, so it's perfectly up to date.

ROS has a huge active community in Academia and(!) Industry (ROSIndustrial), many robotic companies use it in their products and ROS2 is currently under development.

A lot of ROS-packages have been written by people during their studies which have moved on later, so they dropped the support for the specific package. But the core components are continuously updated (although the focus is now more on ROS2).

ROS is not going to be replaced soon, so it's a save bet to use it.


问:
How to attach flysky receiver to racerstar all in one esc and flight controller?
I am building an fpv racing quad I encountered the following problem:

I have a Racerstar StarF4s 30A esc and flight controller combo and a Flysky 2.4ghz iA6B receiver how and where do I connect these together?

答:
Let me assist you and point you in the correct directions:

the manual for your Racestar FC and ESC is here (it is in picture form):
https://www.racerstar.com/racerstar-starf4s-30a-blheli_s-dshot-4-in-1-esc-aio-f4-osd-flight-controller-w-or-bec-current-sensor-p-195.html

What you want to do is use the PPM out from the Flysky iA6B, connect it to the PPM in of the Racestar FC shown on the connection page. There might be other options but this will be the easiest one i personally think.

for cheapish components like these there is usualy no manual and when they are not sold anymore you are buggered (if you did not safe the description images which showed how to use them). I can suggest you to save those instructions somewhere for your own good.

As many who are in the RC hobby may know, maybe this site is not the best for getting help with this type of problem (grown up robotics people tend to have opinions on petty RC-hobbyists) youtube (RCModelrewiews, Joshua Bardwell, ...) and Oscar Liang  or sites may be more informative.


问:
Powering multicopter with multiple dedicated batteries
I am in the starting phase of building a fairly large octo-copter drone. I am currently thinking about the possibility of powering my octo-copter design using 4 separate batteries where each battery powers 2 motors each, in order to get around the problem of the large current that would pass through the single central line (causing heat and voltage drop etc.) if I where to connect all batteries in parallel.

For sure there would always be a slight voltage difference between each battery and if you keep flying for too long you are gonna get one battery that runs out before the others, but I am theorizing that it shouldn't really matter as long as I make sure never to completely exhaust any of the batteries. The flight controller should compensate for the lower voltage provided to each engine. That is my theory at least. I saw a video of these guys doing the Hobbyking Beer-lift challenge with a really large drone, and they seemed to be using something that seems to be 4 separate circuits, one for each motor. Has anybody done something similar to this, or have any experiences about if it's a good idea or not?

答:
I'm a little late to this one but i think there is still valid points not made:

The idea is ok, but i would not consider it sound, primarily because of safety reasons. If one battery fails or gets too low you still lose the whole platform.
As far as i know there is no autopilot which compensates for more than one random motor/esc failure.

The safe way for big drones is using high capacity Schottky diode to make redundant battery harnesses, which prevent over-draining or failing batteries to be loaded by the good batteries.

Like this you have the advantage of monitoring your batterie performance over telemetry because you can see the combined voltage level as one. In your suggested separate case you would need to monitor each voltage which is quite a burden.

Personally I don't think you save a lot of weight by not using a central PDB which you can get for very high voltage ratings, the wires may be chosen a little thinner but if you go octocopter you go large anyway, so i think safety is more important than 200g of extra weight.


问:
Are there solid state directional sensors available?
I have no problem building little robots which are more or less self driving RC cars.  Now I want them to be aware of and interact with each other.

In simulations they all know the position of all the other robots.  I would like to get something similar working in the real world if possible.

Are there sensors available that will tell me robot A is 0.8 meters away 78 degrees from my right and robot B is 1.2 meters away at 113 degrees from me left?

Direction is more important than distance.

I can sort of get something working with a bluetooth power level sensor on a continuously rotating servo but I'm hoping there are better methods already in use with swarm robotics, although I haven't found any.

Thank you for any insights or leads.

答:
This isn't something that can just be answered. There are numerous ways to do this.

Low Tech:

You can go low tech with IR emitters and retroflective tape (or retroflectors) with IR sensors.

High Tech:

A more high-tech solution would be to utilize ultra-wide band transceivers. With UWB you can pinpoint your rovers in 3 dimensional space around each other.

You could also use radar, lidar, or cameras and then train an adversarial neural net system to "visually" discern what a rover looks like -- with the size of the rover in the visual system you could estimate distance.

I would advise just researching differing solutions for ranging and detection and put some thought into what your realistic budget and capabilities are.

Hope this helps.


问:
Autonomous car steering using IR sensors
I want to steer a RC car in a straight line.The car has 4 sharp IR sensors on each corner of the car to help it steer the corridor.The corridor is irregular and looks something similar to the picture below.

The car needs to be stay exactly at the middle(shown by lighter line) and take help of the IR sensors to correct its path.

The car has a servo on the front wheel to steer and another that controls the speed.

I tried running it using a algorithm where it summed the values on each side of the car and took the difference.THe difference was then fed to a pid control the output of which went to steer the car.The greater the value from the pid (on either sides), the greater the value of the steering angle till it reaches the middle.

It works for the part where the walls are at similar distance from the center and even then it oscillates a lot around the center and fails miserably around the bumps in the corridor.

I need to make changes to the algorithm and need some help in steering me  in the right direction.

The IR sensors are too finicky and is there a way to filter out the noise and make the readings more stable?

Any help regarding the changes that needs to be implemented is much appreciated.

Currently the car only uses 4 IR sensors to guide.I can also use 2 ultrasonic sensors.

答:
If you have a map, then you can use a Kalman filter with a motion update based on odometry and a sensor update based on the actual IR sensor readings and the expected sensor readings given your current position and the map. The PID control you implemented will only be able to make the vehicle drive in the actual center of the hallway, which would be a curved path based on the bumps in the hallway.

HOWEVER, the simplest things you should try are as follows: 1) Implement an averaging filter on the IR sensor readings...aka take the average of n IR readings as the input to your control algorithm. This simulates a low pass filter and can eliminate high frequency noise. 2) Implement a threshold filter on the IR sensors...if the readings are greater than some range value, then DON'T use that sensor in your control loop. Combined, an averaging filter and threshold filter should mean that your PID loop is only controlling to the center of the walls that you want. In the image below, the algorithm would only control based off readings from the red walls, ignoring any readings from the black walls. If you don't find any walls within your range threshold (i.e. the hallway is open on both sides, you can try just maintaining your current heading).


问:
Feasibility of SLAM with heterogeneous sensors
Why are there so few published  implementations of SLAM with support for heterogeneous sensors?

Heterogeneous sensors include LIDAR, monocular cameras, stereo cameras, thermal sensors from more than one manufacturer on one or multiple robots.

I am concerned that the lack of availability is not just a matter of time, effort, and politics but also of mathematical/algorithmic feasibility.

答:
I would somewhat disagree with the statement of lack of heterogenous systems. It is pretty common to always combine a camera sensor with an IMU. LIDAR + Camera has also been a pretty popular idea over the last few years.

In regards to lack of more exotic sensors e.g Thermal, RADAR I would say it is due to the fact that the main purpose of SLAM is to build 3D maps. The best sensors to give you 3D information is LIDAR and Cameras so the majority of focus has been on those two systems.

A couple of other reasons for lack of heterogeneous sensor systems:

Cost:

Adding more sensors, especially more exotic ones like Thermal cameras adds to the cost of your sensor platform.

Computational Complexity

More sensors means more computational cost. Not only in the backend portion of fusing the sensor information, but also your system requires now multiple frontends. This becomes especially prevalent on small robots like UAVs, where computation is a premium.

Lack of benefits

The sensors have to complement each other. A LIDAR + Stereo Pair is not a good combination, as the purpose of the Stereo Pair is made redundant with the LIDAR.

Form Factor/Availability/ Ease of Use

The sensors could be too big to put on a small vehicle, getting access to good ones could be difficult. They may require exotic connectors, or propriety software libraries. A bunch of different reasons that make it too complicated to use compared to a simple camera.

On the topic of algorithm feasibility the backends used in Sensor Fusion such as Kalman Filters and Factor Graphs are abstract enough to allow any combination sensors. So this is not an issue.


问:
Sizing servos for robotic arms
I am designing a robotic manipulator. The basic design is shown below. I am trying to calculate the torques around each joint to choose servo motors. I have made an attempt to find the torque around the two pitch joints for the two links (although I'm not fully sure that this is correct).

However I do not know how to find the torque required for the base rotation joint. How would this be calculated?

答:
I recently built a robotic arm using 5 servos.Here is my thought process-

Torque can be thought as angular force. As we know W = mg.Ignoring friction the minimum force required to move an object in the z direction(vertically), at least force greater than weight is needed.Let's say an object weighing w is placed on a friction-less surface.
In that case any amount force can move it, the acceleration will vary.(F= ma,applied force  /mass = resulting acceleration )

In case of the first revolute joint ,in theory any torque should do.But if the moment of inertia is very high it might burn the motor.

but for the second revolute joint minimum torque required = (mass of the first link * half of the length of first link) + (weight of the third servomotor * length of the first link) + (mass of the second link * (length of first link +  half of the length of second link)) + (weight  at the end-effector * (length of first link + length of second link)).

And for the thrid revolute joint minimum torque required = (mass of the second  link * half of the length of second link) + (weight  at the end-effector * ( length of second link)).

use S.I. units in all cases.

Title: Sizing servos for robotic arms


问:
iRobot Create 2 opcodes, data bytes and Realterm
I recently purchased an iRobot Create 2.
To connect to it, I am using RealTerm
Although I can send opcode commands that do not require a Data Byte (128, 173, 133, etc), I cannot figure out the format for opcodes with data bytes

For example, I want to turn on the power LED to full brightness.
I have tried the following formats, but to no avail:

139,4,0,254

139 4 0 254

139004000254

13940254

Any direction is appreciated.

答:
Found the answer to my own question.
The iRobot needs to be in Safe or Full mode in order to control the LEDs.
So, I was entering the numbers correctly (139 4 0 254), but needed to send the command for Safe (131) or Full (132) first.</p>


问:
Control loop in or outside of main loop
I've been working on some small projects for fun to learn more control theory things and embedded programming. Thank the esp32 and Arduino!

Either way I've been programming everything entirely in a single loop, for a bit of pseudo code:

```<code>void loop(){
    data = get(sensor);
    pdata = process(data);
    lqr(pdata);
    millis(10):
}
</code>```

This above code is by no means real code, just a quick example of how my code is designed. I am by no means any kind of programming expert and only really know Arduino styled Wiring and bits of other things to complete small tasks.

What I'm unsure of, is it better to buffer/loop sensor data as fast as the micro-controller can and then afterwards only call the controller (LQR in this case) to actuate the system at my calculated discrete period like so:

```<code>void loop(} {
    unsigned long currentMillis = millis()
    interval = 10;
    data = get(sensor);
    pdata = process(data);

    if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis;
        lqr(pdata);
    }
}
</code>```

or call everything sequentially as I have been doing.

I feel as if having everything in the loop being calculated and then actuated sequentially adds extra time (if only microseconds the esp32 is 200~mhz or so) to the loop which isn't accounted for in my controller.

What is the best method/style for programming an embedded controller. Are both my methods/ideas incorrect?

答:
In simple case it probably doess not matter, but if you have more (and slow) sensors as well as more things to do, the second approach is better, because you can let do sensors do their work (in their HW and their own computing power), collect ressults when they are avaiable and in meantime do many other things.

(Also pseudocode:)

```<code>#define sensors 100
#define devices 50
....
void setup(){
   for (int i =0 ; i<sensors; i++){ start(sensor[i]);};
   for (int i =0 ; i<devices; i++){ open(device[i]);};
   ....
   drone.begin();
}
void loop(){
   for (int i =0 ; i<sensors; i++){
        if (sensor[i].have_data()) {
            data.update(i,sensor[i].read_data());
        };
   };
   for (int i =0 ; i<devices; i++){
        if (device[i].is_free()) {
             device[i].display_next_funny(message[i]);
        };
   };
   if (data.are_updated()) {
      drone.update_path(data);
   };
   if (drone.not_there_yet()) {
       drone.follow_planned_path();
   };
}
</code>```

Now your drone is following planned path as fast, as it can, while you read all sensors as fast, as they have some new data for you and show funny messsages on displays, play sounds, blink LEDS etc as fast, as possible.

Does not matter, if sensor[0] can provide data 100x per sec and sensor[1] only every other second, does not matter, if LED goes on/off instantly and servo with "radar" need two seconds to make full turn.

You just read sensor, when it is available, send new message, when there is free buffer and still you can let your drone react hundred times per second (if there is somehing to do).

You will probably spend a lot of time just empty cycling the loop(), while waiting for new data arrive, all messages to be displayed and for your drone to move its slow mechanical motors to next step, but it is OK, as you have nothing better to do anyway.

On the other hand you can react nearly immediately to any event and still make funny effects in free time.

Title: Control loop in or outside of main loop

Body:
In simple case it probably doess not matter, but if you have more (and slow) sensors as well as more things to do, the second approach is better, because you can let do sensors do their work (in their HW and their own computing power), collect ressults when they are avaiable and in meantime do many other things.

(Also pseudocode:)

#define sensors 100
#define devices 50
....
void setup(){
   for (int i =0 ; i<sensors; i++){ start(sensor[i]);};
   for (int i =0 ; i<devices; i++){ open(device[i]);};
   ....
   drone.begin();
}
void loop(){
   for (int i =0 ; i<sensors; i++){
        if (sensor[i].have_data()) {
            data.update(i,sensor[i].read_data());
        };
   };
   for (int i =0 ; i<devices; i++){
        if (device[i].is_free()) {
             device[i].display_next_funny(message[i]);
        };
   };
   if (data.are_updated()) {
      drone.update_path(data);
   };
   if (drone.not_there_yet()) {
       drone.follow_planned_path();
   };
}

Now your drone is following planned path as fast, as it can, while you read all sensors as fast, as they have some new data for you and show funny messsages on displays, play sounds, blink LEDS etc as fast, as possible.

Does not matter, if sensor[0] can provide data 100x per sec and sensor[1] only every other second, does not matter, if LED goes on/off instantly and servo with "radar" need two seconds to make full turn.

You just read sensor, when it is available, send new message, when there is free buffer and still you can let your drone react hundred times per second (if there is somehing to do).

You will probably spend a lot of time just empty cycling the loop(), while waiting for new data arrive, all messages to be displayed and for your drone to move its slow mechanical motors to next step, but it is OK, as you have nothing better to do anyway.

On the other hand you can react nearly immediately to any event and still make funny effects in free time.


问:
problem of power on PIN 1 and PIN 2 of DIN connector of Roomba 521
I notice there is a problem of power on PIN 1 and PIN 2 of DIN connector of Roomba 521...I have only 0,5 V but it's expected 12V...motherboard damaged?</p>

答:
I find right answer:

with connected broken buck converter I measured 0V, but removing broken buck converter all work fine and I can measure 12V.


问:
Camera mounted on robotic arm - XYZ mapping
I have a 5 axis robotic arm that has a depth camera mounted near the toolhead on the left side.

I have object detection software running on the camera and can find the XYZ of any object in mm that the camera sees relative to the middle of the camera.

The robot can move to any XYZ position in mm relative to the base mounting plate of the robot.

I'd like to get the arm to move to the position of an object seen by the camera. How would I go about doing this? Is there a software library for this?

答:
The problem is solved with just a straightforward application of multiplying the transformation frames.

Given:

P: 3D point in camera frame

T_to_C: Transform from Camera to toolhead

T_B_to: Transform from toolhead to Base(your base mounting plate)

PB: Point in Base frame

Just multiply them through PB=T_B_to * T_to_C * P

You now have the XYZ of your Object in the Base Frame. Just calculate the inverse kinematics of that position to move your arm.

Notes:

The transform between the toolhead and the camera is sometimes called the hand-eye calibration. If you don't have this then you must calculate it through a calibration process. ROS has a bunch of different packages for it.

Your transform between toolhead and base may be composed of multiple different transforms between the various joints in your robot arm.


问:
What's the difference between Pose Measurement and Position Measurement?
I have read a robotic related paper called Estimating SE(3) elements using a dual quaternion based linear Kalman filter, and the author proposed that the equation of position measurement is denoted as:

$\mathbf{a}=\mathbf{R}\mathbf{b}+\mathbf{t}$

where $\mathbf{a}$ is the sensor measurement, $\mathbf{R}$ ∈ SO(3) is the rotation matrix, $\mathbf{b}$ ∈ $\mathbf{R}^{3}$is the point to be transformed and $\mathbf{t}$ ∈ $\mathbf{R}^{3}$ is the translation vector. In an application such as rigid registration of images, $\mathbf{a}$ is the sensed location of points and $\mathbf{b}$ is the corresponding point on the CAD model of the object.

The equation of pose measurement is denoted as:

$\mathbf{AX-XB=0}$

where $\mathbf{A,B}$ are pose-measurements, and the $\mathbf{X}$ is the desired transformation to be estimated.

So is there anyone who know what the difference between pose measurement and position measurement?

答: what you are looking for is written in the paper. Position refers only to x,y,z translational measurements while pose means position and orientation. </p>


问:
Very high value readings for light bumpers
I am reading out the values for the light bumpers on the Icreate2 robot. According to the manual, these values should be in the range 0-4095.

Whenever I read out these values, they start out as well-behaved values in the expected range. However, after a minute or so, the values jump up to very high values >20,000, for example.

I have attached a screenshot of a python-based utility I wrote to show the values of all sensors. You can see that some bumper values have very large values. Nothing is in front of the robot.

Does anybody have any idea about what's going on?

答: I can only think there is something wrong with the firmware or the hardware. Maybe you can try to reupload the firmware or update it. </p>


答: I would check to make sure as the program runs it’s not miss interpreting bits. If you use the Stream OP Code (148) it produces a check sum. I would Change to stream and verify that you still get >20,000 with a correctly computed check sum. If you are getting correct check sums and high values post your code it could be something on how it’s being interpreted over time. </p>

Title: Very high value readings for light bumpers

Body:
I am reading out the values for the light bumpers on the Icreate2 robot. According to the manual, these values should be in the range 0-4095.

Whenever I read out these values, they start out as well-behaved values in the expected range. However, after a minute or so, the values jump up to very high values >20,000, for example.

I have attached a screenshot of a python-based utility I wrote to show the values of all sensors. You can see that some bumper values have very large values. Nothing is in front of the robot.

Does anybody have any idea about what's going on?


问:
Hardware for Industrial-Grade CNC
I have developed a prototype of a CNC Machine with Arduino and Raspberry Pi for my first client, I now need to use industrial grade components, but I struggle with the hardware, I am confused.

I just learned about PLC, and I don't know anything about them.
What it takes to make an Industrial-Grade CNC ?

Should I use a PLC for my CNC ?
What PLC to use ? What modules ?
What is the hardware configuration ?

I want to learn,
But I struggle to find informations for this on the web.
Do you have some documentations ?

I need your help, I am a little bit lost I have to admit.

Thanks !

答:
Arduino and RPI can take the role of control systems hardware. They are not what someone would call industrial grade (although, in some cases, they are used in industrial context).

It is hard to define what industrial grade means, it is in many cases subjective, but here are some aspects which I think are mostly agreed upon:

Hardware and Software interfaces compatible with industrial equipment. Industrial equipment communicate to each other using specific protocols like CanOpen, Sercos, EtherCAT, etc. These require a hardware interface and a well suited communication stack (on the software side)

Usable in an industrial setting (housing, rail-mountable, screw connector, no soldering needed for use, UL or CE certification (depends on country), 24V DC or 110V/230V/400V AC - depends on country)

Programming by the end-user. Industrial control systems are programmed by the system integrator or by the end-user. They need to be familiar with the programming language. CNCs are programmed in some G-Code dialect, PLCs have dedicated programming languages like Ladder diagram, Structured Text, etc. This is the application program, similar to a script, running on the PLC, CNC or RC (Robot Controller) on top of the manufacturer provided "firmware".

Technical support is required, since the end-user might have questions or problems in the exploitation of the system and if production stops at the end-user that can be costly. End-users want to be sure that there is long-term technical support available.

PLCs are used for controlling industrial systems, but they are not the first choice for controlling synchronized motion (this is somewhat changing with the PLCOpen Motion Blocks for very simple applications, not for machining applications). CNC-controllers are the first choice to control synchronized motions. CNC-controllers also include PLC functionality for dealing with non-motion related tasks (e.g. switching coolant on or off during machining).

A high-end industrial grade CNC Controller would be a Siemens CNC Controller, a Beckhoff Controller with a CNC Module, Mid-range e.g. Fagor CNC Controllers. On the very low-end (low-cost) side a CNC controller like the Mach3 or Mach4 might (!) pass as industrial grade, depending on the client requirements. As for open source there is LinuxCNC, which migh(!) pass as industrial grade,  and the GBRL project which will probably not pass as industrial grade, due to its arduino target. Again, what is and what is not industrial grade is somewhat subjective.

CNC Controller communicate with drive amplifiers. Some examples here are overlapintg with the ones above (Siemens, Beckhoff, Bosch, Lenze, ABB). Most of these also have matching motors for the drive amplifiers.

As for mechanical hardware, gearboxes (e.g. Nabtesco), linear guide rails (e.g. SKF).

Usually all companies mentions have pre-sales technical support to assist you in picking the right equipment for your application. Please note that brands in my answer is biased towards the EU market. There might be other comparable brands well known in other geographical regions)

Title: Hardware for Industrial-Grade CNC


问:
Robot localization in a known map without knowing its initial position in that map
Firstly I would like to say that I'm no expert in Bayesian Filters such as Kalman Filter and Particle Filter, but I've used the EKF before in a robot that has both wheel encoders and an IMU to localize itself in a map such that it knows its initial position in the map and it worked like a charm. Now I want to build a robot that uses the same sensors as above in addition to a vision sensor (be it a kinect or some kind of a 2d Lidar) that localizes itself in a known map but has no idea of its whereabouts when it starts; I heard that the Particle Filter is used for that kind of work so I have 2 questions:

1- Can I achieve the same thing using just the Kalman Filter?

2- Can I use both of them so that the particle filters will be used until the robot has the highest certainty of a location then "turn it off due to its high computational cost" and feed that location into an EKF as an initial estimate?

答:
Answers to questions:

1) Yes you can use a Kalman filter. The trick to doing it is setting the initial covariance to be big enough to cover the whole map.

2) This is also a viable approach. I would probably keep them separate as you described. One advanced trick you could do is that depending on the implementation of the particle filter each particle is an EKF. Once you reach a high enough certainty you could maybe just continue working off of that one particle. The separate approach ,however, sounds a lot easier and is probably the one you should use.

The reason that a particle filter is better for localization over an EKF is that it allows for multiple hypothesis. As you begin to acquire data you could have 3 separate valid locations in the map. The EKF approach will generally force you to only consider 1 of them as valid. If more evidence comes along later that invalidates that choice then you are screwed.


问:
What is the best way to simulate a robotic arm?
I have been working on a senior design project. In this project, we designed a 4-DOF robot arm. Now, I want to do a simulation for this system. I guess I have to design controllers for the servo motors. And, combine those with my simulator. What are your thoughts about this? How should I do the simulation? What programs, tools,  or frameworks should I use? I have been thinking to use MATLAB Simscape and SimMechanics for simulation and Simulink for the design of the controller. Also, one other option for the simulation is the MoveIt framework.

Thanks for your opinions.

答:
Try to use Gazebo.
http://gazebosim.org/
It has an interface to ROS which makes development of control software easy. and note that ROS is only usable on LINUX.</p>


问:
How to retrieve parameters from mavlink .tlog using pymavlink?
I've been able to use pymavlink.mavutil to read telemetry from a .tlog created by MissionPlanner.

To do this, I create a mavlogfile like this:

mlog = mavutil.mavlink_connection('mylogfile.tlog')

Now I want to read the flight parameters (settings) from the .tlog . The method mavlogfile.param_fetch_all() appears to be designed only to work with a live telemetry link rather than a log. It sends a parameter request command, which obviously has no result when you are linked to a log rather than an actual aircraft.

I know the parameters are in the .tlog... how do I get them out?

答:
Turns out it's implemented in https://github.com/mavlink/mavlink/blob/master/pymavlink/tools/mavparms.py

and the relevant code is:

m = mlog.recv_match(type='PARAM_VALUE')


答: Try to use V-REP, it is very straighforward to use and got a MATLAB and ROS interface. Is usable in WIndows, MAC and Linux. </p>

Title: What is the best way to simulate a robotic arm?

Body:
I have been working on a senior design project. In this project, we designed a 4-DOF robot arm. Now, I want to do a simulation for this system. I guess I have to design controllers for the servo motors. And, combine those with my simulator. What are your thoughts about this? How should I do the simulation? What programs, tools,  or frameworks should I use? I have been thinking to use MATLAB Simscape and SimMechanics for simulation and Simulink for the design of the controller. Also, one other option for the simulation is the MoveIt framework.

Thanks for your opinions.


问:
Sensing torque of stepper motor
I need to sense the torque of a stepper motor at any given time. I'm planning to tension a wire by using a stepper motor, and in order to keep the wire at a fixed tension I would need to know the tension at any given time. Based on this I can turn the stepper motor to achieve the desired torque.

If this is not possible, appreciate some alternatives on how I can achieve this. I've looked into strain sensors as well, but not sure if this works.

答: Sensing the torque in a stepper motor is not a straightforward task. A method named Field Oriented Control is used to control the torque in a Stepper motor and several companies supply stepper motors with integrated controllers as Schneider and Nanotec. You can also look at mechaduino where they are also implementing a torque control on stepper motors, which they also try to align the energize of the coils to the rotor orientation. An external torque sensor I believe would be more expensive. </p>

Title: Sensing torque of stepper motor


答: The Simulink and SimMechanics combination is a well suited one. You can add further SimScape elements for motors ot similar elements are needed. There is a Matlab demo on simiar topics here and here.</p>


问:
How does one estimate motion from sparse encoders and IMU-Gyro data?
I have a robot which has (for simplicity) only one degree of freedom; it rotates about the z-axis (think like a crane), and has no slip.

The shaft encoder delivers data in 0.1° increments, so when measured at 30 Hz, there is usually a succession of identical values until the value jumps to the next 0.1°.

I have an IMU with a Gyroscope that I can place somewhere on the robot (not at the center of rotation).

I would like to update estimates of the current angle of rotation at greater than 30 Hz.

How would one go about fusing this very coarse encoder and IMU data? I'm assuming EKF filtering will be part of the answer.

I would be very satisfied with canonical literature on this topic as well.

答:
When you have one fast-moving source and want to fuse it with a slow-moving source, a complimentary filter should be sufficient. Hopefully, it's a lot easier to understand than Kalman filters.

There are plenty of examples where they use a complimentary filter to combine accelerometer and Gyroscope. When you say the Z-Axis, I assume you mean vertical axis and can't use the accelerometer to work out gravity. It doesnt matter where you place the gyro, the angular velocity is independent of the origin

Gyro gives a measure of angular velocity, and from that, we calculate the position of the crane. However, there will be noise on that signal which will gradually cause the position to be wrong.

In a drone, the accelerometer can be used to get gravity direction i.e. down. However, when the drone is moving, we can't use it because it's measuring the acceleration due to the movement. You can think of that like the inaccuracy like the shaft encoder. Though it's not noisy like an accelerometer, it is inaccurate in the short term, but stable long term.

These two sensors/measures of turn can be turned into an angle and combined. The Gyro/fast-moving source will go through a high-pass filter and the stable/slow shaft encoder can go through a low-pass filter and then added.

I enjoyed watching this video from Brian Douglas about the topic.


答:
Just a guess: 4096 consists of two bytes, maybe a wrong ordering or a big/little endian thing? (Sry, cannot comment due to to less reputation)

Edit: Basically what Pete already said...


问:
How to handle landmarks in Bundle Adjustment?
At the moment I try to understand Bundle Adjustment on a level to be able to implement it on my own. The major point I'm still confused at is the management of the landmarks. So, let's think of a few frames, that all share  some landmarks with each other (like in the image below).

I can triangulate the same corresponding feature in image 1 to image 2, from image 2 to image 3 and so forth. However, I get (slightly) different results for the same point in the world. I first tried it with fusing all the landmark estimates from the different triangulations (see an older post). But, I'm not sure if that is really the way to go.

If my question is still a bit to unclear, please let me know and I will improve it. I'm looking forward to interesting answers.

答:
First thing to do is pick your World frame. This is your main frame where the 3D coordinates of your points and poses of your cameras will be. Typically the origin of this world frame is the pose of your first camera. You then triangulate every point with all of the cameras that see it. A rough guess can be computed with an algorithm like DLT, and then refined with some non-linear optimization. Some source code for this can be found here(notice it has from N views triangulate). Make sure the output for this is actually in the world frame and not relative to some camera.

Now put all of your camera poses and 3D points in a bundle adjustment problem. You have 2 sets of variables. Each landmark is going to be connected to every camera pose that views it via the reprojection error. Solve it using a non-linear least squares solver like Ceres. They even have a basic tutorial with some given 3D points and camera poses, and reprojections.


答:
referring to your equation
$$ \boldsymbol{a}=\boldsymbol{Rb}+\boldsymbol{t} ,$$
the position measurement would be $\boldsymbol{t}$ whilst the orientation measurement would be $\boldsymbol{R}$.
These quantities can be obtained in different ways, e.g. position could be measured using GPS while orientation could be obtained using an IMU.
I hope this helps,

P.S. feel free to ask further details, I've written my master thesis about a similar estimator.

Title: What's the difference between Pose Measurement and Position Measurement?

Body:
I have read a robotic related paper called Estimating SE(3) elements using a dual quaternion based linear Kalman filter, and the author proposed that the equation of position measurement is denoted as:

$\mathbf{a}=\mathbf{R}\mathbf{b}+\mathbf{t}$

where $\mathbf{a}$ is the sensor measurement, $\mathbf{R}$ ∈ SO(3) is the rotation matrix, $\mathbf{b}$ ∈ $\mathbf{R}^{3}$is the point to be transformed and $\mathbf{t}$ ∈ $\mathbf{R}^{3}$ is the translation vector. In an application such as rigid registration of images, $\mathbf{a}$ is the sensed location of points and $\mathbf{b}$ is the corresponding point on the CAD model of the object.

The equation of pose measurement is denoted as:

$\mathbf{AX-XB=0}$

where $\mathbf{A,B}$ are pose-measurements, and the $\mathbf{X}$ is the desired transformation to be estimated.

So is there anyone who know what the difference between pose measurement and position measurement?


问:
Articulated configuration
I'm reading Theory of Applied Robotics, 2nd edition by Jazar. The author states that articulated configuration has the following form: $R \vdash R \perp R$ where $R$ stands for revolute joint, $\vdash$ represents that 2 adjacent joint axes are orthogonal (intersect at right angle) and $\perp$ represents that 2 adjacent joint axes are perpendicular (they close right angle with common normal). The example of articulated configuration is given in the image

So by looking at the image axes $z_1$ and $z_2$ are supposed to be perpendicular but they look parallel to me. What am I missing ?

答:
Seems that the book has an error. You are right, they should correct that. </p>

<p>In the drawing it can be seen that those two z-axis are indeed parallel. I recommend that you considered them parallel and try to draw them to see that it fits with the image. </p>

Title: Articulated configuration

Body:
Seems that the book has an error. You are right, they should correct that.

In the drawing it can be seen that those two z-axis are indeed parallel. I recommend that you considered them parallel and try to draw them to see that it fits with the image.


问:
Diagnosing a lower torque on one motor of a Zumo Robot
I have a Zumo Shield. It is small and tracked, and contains two micro metal gear motors soldered to it.

In theory, these motors are identical and do seem to rotate at the same speed. However, the left side seems to have considerably less torque than the right. The left side will stall on small obstacles and the robot ends up veering off course.

Another test of turning force, if I set both motors to spin at the same speed, while the robot is off the floor, I can put my finger on the track and cause the left to stop with a lot less pressure than the right.

I know it's never going to be perfect and I need some sort of feedback loop will be required to keep it straight however there is a considerable difference between the two.

What is the likely cause of this difference? Soldering? Motors? Gearing? Motor drivers?
How can I go about diagnosing and therefore fixing this problem?

答:
This is not a complete answer, but one generic technique for diagnosing differences between devices which should be identical is divide and conquer. Swap items until you see where the problem follows.

For instance, if you swap the motors and the problem stays with the motor, you probably have a faulty motor. If a different motor with the same amplifier exhibits the same problem, it's probably the motor driver. If swapping them and neither of them works properly, or both work, it may be (or have been) the soldering.

Once you have identified the subsystem which is at fault, you can then continue to investigate and find a more specific subsytem or component.

For instance, if it appears to be the motor drivers, you can try swapping the amplifier chips. If the problem follows the chip, replace the faulty one, if both chips work as well as each other, try measuring voltages across the chip, current flowing through the chip, the resistances seen between the pins of the chip and the motor and power supply connectors etc.

Similarly with the motor module, you can try swapping wheels (as you've done) gearboxes, encoders (if they have them) and so on.

The key is to build a hypothesis about what might be wrong, conceive of a test which can prove (test) that hypothesis and then use the data from that test to either identify the component which needs to be fixed, replaced or investigated next.

Rinse and repeat until the root cause is found.


问:
Make robot drive as far from obstacles as possible
I am trying to make robot drive around the room, avoiding obstacles with pathfinding. The problem is I get few waypoints from pathfinding algorithm for robot to drive to (green dots), but it drive a bit too close to corners. I try avoiding that by using blur and threshold, to make walls thicker for the algorithm, but it causes problems with different size walls(as in example picture, blur is these black dots). How do I make my robot drive in the middle as possible of room, to avoid walls and be as far as poosible (the orange line) from the obstacles?

答:
Some hints:

Use Minskowski sums to "grow" the obstacles taking into account the shape/orientation of the robot.

Not easy to understand and implement, BUT already implemented in geometrical algorithm libraries.

After finding the path, postprocess the output. Try to use a smoother for the path, like voronoi diagram in order to go to the middle of the corridor.


问:
Usage of Multibeam 2D Imaging Sonar for AUVs, testing them in the pool environment
I belong to an AUV team at my university. We are planning to have a Multibeam 2D Imaging Sonar (the Blueview P900) for our AUV to detect obstacles underwater.

I have the following questions to ask on the feasibility of testing/implementing such sonars on AUVs.

As we know that these multibeam sonars have multiple reflections arriving at different times from various surfaces while testing in a pool, is there any recommended way to filter these noises in the image obtained from the sonar pings?

Are such sonars in use/test by any other team/organization anywhere else who do pool testing other than a ocean/reservoir testing where multiple reflections are almost zero except the reflections from the obstacle(s)?

Also i would like to know the recommended image processing algorithms that can be implemented/used to detect obstacles from the sonar images.

答:
As you've noted, a pool is one of the worst environments in which to test acoustic sensors; there is nothing to dampen the echoes, and the multipath is quite extreme.

Generally, one of 3 things is done to help mitigate these effects -- the goal being an anechoic test chamber.

Make the shape of your test tank such that all echoes are reflected into a trap.  The TRANSDEC anechoic pool accomplishes this with its eliptical shape; sounds that hit the bottom are directed toward the edge, which has a "trap" around its perimeter. (I can no longer find the cut-away diagram for that... sorry).

Put foam wedges along the outside of the tank to absorb acoustic energy, similar to the way that recording-studio anechoic chambers are designed.

Aerate the water to scatter the echoes, a technique described in this paper: "Anechoic aquarium
for ultrasonic neural telemetry".  The use of microbubbles is apparently able to prevent most of the multipath effects.


问:
Localisation of Robot in a Rectangle
I am wondering how to go about localising a robot in a rectangle drawn on a smooth surface. Basically, there is a tape that lines the shape of the rectangle, and the color of this tape is unique in the scene. I have a camera mounted on the robot. So my question is, is it possible using image processing, and knowing the size of the rectangle, to localise the robot within this rectangle so it may navigate the area of the rectangle. Obviously the robot is driving in the same plane as the rectangle. Thanks</p>

答:
If you know the color and also how long the line is then, you can detect it (easy color detection with opencv) and estimate the distance between the line and the camera.

Basic concept you find how many pixels the line is occuping on the picture, you can find the focal length of your camera (from the manual) and you know also the actual size of the line in the real world so you can estimate the distance of the camera to the line.

as you can see in the picture

Title: Localisation of Robot in a Rectangle


问:
Computing the Jacobian matrix for Inverse Kinematics
When computing the Jacobian matrix for solving an Inverse Kinematic analytically, I read from many places that I could use this formula to create each of the columns of a joint in the Jacobian matrix:

$$\mathbf{J}_{i}=\frac{\partial \mathbf{e}}{\partial \phi_{i}}=\left[\begin{array}{c}{\left[\mathbf{a}_{i}^{\prime} \times\left(\mathbf{e}_{p o s}-\mathbf{r}_{i}^{\prime}\right)\right]^{T}} \\ {\left[\mathbf{a}_{i}^{\prime}\right]^{T}}\end{array}\right]$$

Such that $a'$ is the rotation axis in world space, $r'$ is the pivot point in world space, and $e_{pos}$ is the position of the end effector in world space.

However, I don't understand how this can work when the joints have more than one DOFs. Take the following as an example:

The $\theta$ are the rotational DOF, the $e$ is the end effector, the $g$ is the goal of the end effector, the $P_1$, $P_2$ and $P_3$ are the joints.

First, if I were to compute the Jacobian matrix based on the formula above for the diagram, I will get something like this:

$$J=\begin{bmatrix}
((0,0,1)\times \vec { e } )_{ x } & ((0,0,1)\times (\vec { e } -\vec { P_{ 1 } } ))_{ x } & ((0,0,1)\times (\vec { e } -\vec { P_{ 2 } } ))_{ x } \\ ((0,0,1)\times \vec { e } )_{ y } & ((0,0,1)\times (\vec { e } -\vec { P_{ 1 } } ))_{ y } & ((0,0,1)\times (\vec { e } -\vec { P_{ 2 } } ))_{ y } \\ ((0,0,1)\times \vec { e } )_{ z } & ((0,0,1)\times (\vec { e } -\vec { P_{ 1 } } ))_{ z } & ((0,0,1)\times (\vec { e } -\vec { P_{ 2 } } ))_{ z } \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 1 & 1 & 1
\end{bmatrix} $$

This is assumed that all the rotation axes are $(0,0,1)$ and all of them only have one rotational DOF. So, I believe each column is for one DOF, in this case, the $\theta_\#$.

Now, here's the problem: What if all the joints have full 6 DOFs? Say now, for every joint, I have rotational DOFs in all axes, $\theta_x$, $\theta_y$ and $\theta_z$, and also translational DOFs in all axes, $t_x$, $t_y$ and $t_z$.

To make my question clearer, suppose if I were to "forcefully" apply the formula above to all the DOFs of all the joints, then I probably will get a Jacobian matrix like this:

(click for full size)

But this is incredibly weird because all the 6 columns of the DOF for every joint is repeating the same thing.

How can I use the same formula to build the Jacobian matrix with all the DOFs? How would the Jacobian matrix look like in this case?

答:
As far as I understand your question that you want the Jacobian matrix for the 6 DOF joint.

Let me start with very basics of robotics. You are in the vary initial phase of robotics learning. You need to understand that each joint represent a single DOF either it would be revolute or prismatic joint.

As far as spherical joint is concern, it can be converted in to 3 revolute joint with three mutually perpendicular axis. So, now you have simplified your spherical joint.

Moving forward to Jacobian matrix. It contain  6 rows. First 3 rows represents orientation and last 3 rows indicated position with reference to a particular coordinate system. Each column in matrix indicate a single joint. So the number of joint/DOF you have the same number column you have in Jacobian matrix.

Here is the more clear view to your question:
A single joint never fulfil more than one DOF, because it complicates the joint and precise control will never achieve. Even if we consider hypothetically a joint with more than one DOF, you need to convert that joint into multiple joints with 1 DOF each to simplify the mathematics and solution.

Ideally 6 DOF robot with 6 revolute joint works for majority on the real problems. But as per your question you considered 6 joint robot with each joint having 3 DOF that makes 18 DOF robot. This will give redundant DOF (i.e. 18-6= 12 redundant DOF). So, to reach robot end-effector to any location with any orientation you will have infinite different solutions (solution means rotation of each joint). So solve this kind of inverse kinematics problem you will require iterative method of inverse kinematics.

Hope, I have answered your question more clearly. To learn basic robotics you can refer John J. Craig - Introduction to Robotics Mechanics and Control -Pearson Education, Inc.

Regards,
Manan Kalasariya


问:
How to implement path planning algorithm considering orientation?
I am developing GUI c++ program to test path planning algorithms: A*, Dijkstra, ....etc in occupancy grid map.

I found many open source codes , but I need to modify them to design a GUI for testing purposes since I am trying to develop new algorithm.

I noticed that the c++ implementations (which is not for ROS) do not consider the rotation or the orientation for robot when deciding the next cell or movement, they only use x and y values with up, down, left, right movements.

Is it enough to consider x and y only?

What about the rotation when I want to use my code in ROS? How to consider it in this case?

Thanks a lot in advance,

答:
x,y may not be enough depends on your vehicle model. You need to use Hybrid A* in case you are using car like model. Refers to the following paper.

paper: Practical Search Techniques in Path Planning for Autonomous Driving


答:
Unfortunately, path planning is more complicated to implement than other algorithm within computer science. If the subject would be a simple audio compression algorithm (mp3) or an array sorting (quicksort) technique, it's possible to discuss the details of how to realize a certain algorithm in C++. In contrast, path planning is a problem from the Artificial Intelligence domain which prevents that the algorithm can be realized in a framework or as a library.

For the concrete problem of an orientation aware path planner some papers were published in the past. The common strategy is to use domain knowledge as a heuristic guidance for a sampling based planner. This description means anything and nothing at the same time. A more elaborated starting point in developing an algorithm from scratch is to program only a path planner annotation system which is able to recognize actions of a human user who controls the robot with a joystick. Such a system would detect, if the robot changes it's direction and what the target location would be. Unfortunately, the task is strongly connected to probabilistic robotics and finite state machine parsers which results into a full blown phd thesis writing projects which goes over 3 years and longer.

Title: How to implement path planning algorithm considering orientation?

Body:
I am developing GUI c++ program to test path planning algorithms: A*, Dijkstra, ....etc in occupancy grid map.

I found many open source codes , but I need to modify them to design a GUI for testing purposes since I am trying to develop new algorithm.

I noticed that the c++ implementations (which is not for ROS) do not consider the rotation or the orientation for robot when deciding the next cell or movement, they only use x and y values with up, down, left, right movements.

Is it enough to consider x and y only?

What about the rotation when I want to use my code in ROS? How to consider it in this case?

Thanks a lot in advance,


答: You can have a look at Hybrid A*, a lot more complicated than normal A*, but it takes into account the orientation. Here the paper.</p>


问:
What Battery Voltage to use?
Im currently designing a small robot that uses  2 of these 12v motors, which are manage by the L298N motor controller board, and is being controlled overall by an arduino. Now I have come across an article(Scroll to bottom after clicking link for the schematic of what I am looking at) talking about powering the robot. It talks about using a switching regulator to stabilize the voltage going to the motors, but also has a disclaimer saying switching regulators uses about 1-2v. Now I am currently looking at this 12v li-ion battery.

So one of my question are, will I need to bump up to a 15v battery because of the voltage drop across the switching regulator?

I also want the battery to last as long as possible so efficiency is important.
And when it comes to efficiency, then it seems like it would waste a lot of power to use a voltage regulator to drop the voltage to 9 volts for the arduino (which already has a regulator built in, but only up to 12v). I know with this at least I can add 9v battery for the arduino alone.

So overall, how do I maximize efficiency while ensuring the motors and arduino get the voltage they need

答:
A few things going on here. First of all -- voltage != efficiency (at least not necessarily). Efficiency is really a measure of total power output which is $I \cdot V$ (where I is current). Next, your 12V battery isn't 12V, it's got an operating range (from your link) of 12.6 to 8.4V, and I assume a typical voltage somewhere in the 11s. That means that based on charge level, it'll range from 12.6 (completely full) to 8.4 (completely empty) and for the most part be somewhere around 11.1 (12.6 / 4.2 is 3, which leads me to believe that it's a 3S battery which will have a nominal voltage of 3.7*3 or 11.1). All of that means that if you need exactly 12V, you won't have it most of the time.

The standard way to do this is to get a battery that's at whatever minimum charge level you want is high enough that a voltage regulator still has room to bring it down to a stable 12V. In this case, it would likely mean going to a 4S battery (which would have a nominal voltage of 14.8), and then bucking it down to 12.

Back to the efficiency question - that mostly comes down to the voltage regulator. Linear regulators are very cheap and simple, but very inefficient - their power loss is directly proportional to the voltage drop (so going from 12V to 6V would waste half the power as heat). A switching regulator is generally a lot more efficient (can be high 80s low 90s % efficiency), and can both increase (boost) and decrease (buck) the source voltage.

Finally - the voltage regulator on your Arduino is an LDO, which is inefficient. You're better off using a switching regulator and feeding that into the RAW pin on the Arduino.

Title: What Battery Voltage to use?


问:
In a discrete bayes filter, why is noise in the sensor treated differently than noise in the motion
This question may be hard to explain or understand. I am studying Kalman Filters using this excellent resource. It divides up the process of a Bayesian Filter into two major steps, belief -> prior and prior -> posterior. There is an extended example involving a hallway with doors and a robot (dog) that we are trying to localize.

We start with a 'belief' of where the robot is along the hallway. In the first step we take that 'belief' and incorporate information from a sensor with respect to the environment (e.g. is what is the probability that it is looking at door and from that it comes up with a preliminary probability distribution of where the robot is along the hallway, which is called the 'prior'. Now the robot moves 0 1 or 2 steps. Using that probability distribution, we update the distribution of where the robot might be - this is called the 'posterior'. The cycle is repeated using the posterior of one step as the 'belief' of the next cycle.

Ok so far? Please correct me if I am stating it wrong.

My question is this: the sensing of the door has a probability of being accurate, in other words, the sensor has a certain probability of giving an erroneous reading. And the sensing of the motion also has a chance to be inaccurate, in other words, the robot might think it moved one spot but it really didn't at all.

This may be overthinking it but I want to understand why those two sources of error are treated differently or specially. Why do they have different names (belief, prior, posterior), why do they have to be in that order, why couldn't you have a third measurement which also could have an error factor?

答:
The reason the two sources of error are treated differently is because.. they are different. To some extent, this is a matter of terminology. Imagine you're walking in a room where the lights keep flickering. Let's say you take five steps towards a door in the dark, and you can predict how far you might have walked in meters based on muscle memory - but you're very likely to be off by a bit. This is your belief, or a prior. Your internal conversion of "footsteps->how close I am to the door" is the 'system model', or 'process model'. While this is technically a type of measurement, you are not explicitly using external information. When the lights do flicker on, you might notice you're much closer to the wall than you'd initially thought. You take this measurement, and update your belief: i.e., convert the prior to a posterior. By doing this update, you've effectively reduced the error in your own position estimate. This predict->update cycle is the crux of a Kalman filter.

In your robot scenario, it is very likely that the robot might have very expensive, accurate encoders that can measure ego-motion (proprioceptive sensing) very well, but a cheap camera or a rangefinder that outputs erroneous measurements of the environment (exteroceptive sensing). When trying to describe the error characteristics of a system, it is conventional to use data from a spec sheet or something similar, that affords you some information as to the error thresholds of the measured quantity, and thus, the two error sources could have different variances. Hence, when going through the predict/update steps of the Kalman filter, it is important to describe the error characteristics for both the steps accurately.

And yes, you can have as many measurements as you want, each with a different source of error. In this case, the Kalman filter cycle would look like this:

```<code>predict
update from sensor 1
update from sensor 2
.
.
update from sensor k
predict
update from sensor 1
<so on>
</code>```

Body:
The reason the two sources of error are treated differently is because.. they are different. To some extent, this is a matter of terminology. Imagine you're walking in a room where the lights keep flickering. Let's say you take five steps towards a door in the dark, and you can predict how far you might have walked in meters based on muscle memory - but you're very likely to be off by a bit. This is your belief, or a prior. Your internal conversion of "footsteps->how close I am to the door" is the 'system model', or 'process model'. While this is technically a type of measurement, you are not explicitly using external information. When the lights do flicker on, you might notice you're much closer to the wall than you'd initially thought. You take this measurement, and update your belief: i.e., convert the prior to a posterior. By doing this update, you've effectively reduced the error in your own position estimate. This predict->update cycle is the crux of a Kalman filter.

In your robot scenario, it is very likely that the robot might have very expensive, accurate encoders that can measure ego-motion (proprioceptive sensing) very well, but a cheap camera or a rangefinder that outputs erroneous measurements of the environment (exteroceptive sensing). When trying to describe the error characteristics of a system, it is conventional to use data from a spec sheet or something similar, that affords you some information as to the error thresholds of the measured quantity, and thus, the two error sources could have different variances. Hence, when going through the predict/update steps of the Kalman filter, it is important to describe the error characteristics for both the steps accurately.

And yes, you can have as many measurements as you want, each with a different source of error. In this case, the Kalman filter cycle would look like this:

predict
update from sensor 1
update from sensor 2
.
.
update from sensor k
predict
update from sensor 1
<so on>


问:
Autonomous RC Car Competitions in germany
Together with a friend of mine and my professor, we were able to build a team and come up with our own self driving rc car.

Due to time we didn’t manage to apply for Carolo-Cup, which is a very good competition where teams from all over the country participate with their own self driving cars.

Does anyone know if there is another autonomous driving rc Car competition in Germany that would take place in early phase of 2020 ?

答: You can find more info about many kinds competitions here</p>


问:
Can you have a career in robotics if you hate mechanics?
I'm a first year electronics engineering student. I love almost all the aspects of robotics - the electronics, algorithms, control theory etc. I can't stand the mechanical aspect of robotics though.

Can I have a fulfilling career in Robotics if I hate mechanics but love all other parts of robotics? I'm ready to learn mechanics if I absolutely have to, but would strongly prefer not to learn any more than the absolute basics.

答:
The answer is 'yes'.

A more detailed answer likely depends on how you define "robotics".  But generally, robotics applications are considered to require a very broad spectrum of knowledge.  So while most robotics includes some form of mechanical function, you could easily specialize in artificial intelligence, microcontroller design,  or any number of specialties without ever having to learn advanced mechanical theory and application. I would even say you are at advantage; I would guess, but do not know for certain, that most people are the other way. They enjoy the mechanical real-world aspects of robotics, not so much the abstract topics.

Given what you say you enjoy though, I would have to wonder if "robotics" should be your focus vs. a more general computer engineering or science focus.