问:
Can I model a 1D segway as a cart-pole system?
The equations of motion for a cart-pole (inverted pendulum) system are given as  </p>

<p>$$(I+ml^2)\ddot{\theta}+mglsin(\theta)+ml\ddot{x}cos(\theta)=0$$  </p>

<p>$$(M+m)\ddot{x}+ml\ddot{\theta}cos(\theta)-ml\dot{\theta}^2sin(\theta)=F$$  </p>

<p>However, I want to model a two wheeled segway-like robot with motion constrained to only forward and backward actions (effectively restricting motion in 1D).  I initially thought that I could model such a constrained segway robot by modeling a cart-pole system with a massless cart (M=0).  Is this the right approach to model the dynamics of a 1D segway robot?  Or is there a better model for the dynamics of such a robot in 1D?  </p>

答:
Let's start by defining some of the quantities in the equations you gave:

$I$ Inertia of the pendulum about its center of gravity

$M$ Mass of the cart

$m$ Mass of the pendulum

$l$ Distance between pendulum CG and its pivot

$x$ Displacement of the cart

$\theta$ Angle between a vertical axis and the pendulum

$F$ Driving force acting on the cart

$g$ Acceleration due to gravity

Note that I chose a different sign convention for gravity than in the original question.  In my answer, $g$ has a positive value, while in the original question $g$ must be negative.

Now let's compare this system and to a "segway-like" balancing robot.  The balancing robot's wheels are analogous to the cart, but in addition to translating, they also rotate.  This rotation means that for a given mass $M$, the balancing robot will require more energy to accelerate, since it needs to translate and also rotate.  So instead of zero cart mass, we expect an additional term that corresponds to this additional required effort.

Another difference is that instead of a linear force acting on the cart, we have a moment applied to the wheels, and an equal and opposite moment applied to the pendulum.

To derive the equations for the balancing robot, we need an additional two quantities, and we'll use $T\!\left( t \right)$ for the driving torque in place of $F$:

$r$ Radius of the wheels

$I_D$ Inertia of the rotating components in the drivetrain

And we end up with
$$
\left(I + m l^2\right) \ddot{\theta} - m g l \sin \left( \theta \right) + m l \ddot{x} \cos \left( \theta \right) = - T\!\left( t \right)
$$
$$
\left( \frac{I_D}{r^2} + M + m \right) \ddot{x} + m l \ddot{\theta} \cos \left( \theta \right) - m l \dot{\theta}^2 \sin \left( \theta \right) = \frac{T\!\left( t \right)}{r}
$$

EDIT:

Since the real system isn't as simple as two bodies connected with a revolute joint, here is some guidance on where to "count" the mass and inertia of system components.

If we can assume we've already decided to model the system as two bodies (i.e. wheels/drivetrain and pendulum), then for each component, you need to ask "with which body does this component move?"  Let's say there are two wheels which are secured to the pendulum by bearings.  Let's also say that the wheels are driven by motors acting through gearboxes and there are couplings connecting the motors and wheels to the gearboxes (we'll call the couplings between the wheels and gearboxes "low-speed" and the couplings between the motors and gearboxes "high-speed").

It's fairly obvious that the wheels and low-speed couplings move as a unit.  And it doesn't take much more thought to see that the inner race of the bearings, the internals of the gearbox and the motor rotors all move with the wheels as well.  In your question, you constrain the robot to linear travel, so we will assume that the two wheels move as a unit, too.  When there is a gearbox involved, the inertia of the motor and high-speed coupling (and sometimes the gearbox, depending on the convention used on the datasheet -- read carefully!) needs to be reflected to the opposite side of the gearbox.  The relationship is given by

$$
I_{low-speed} = R^2 I_{high-speed}
$$

So the drivetrain inertia would look something like this:

$$
I_D = 2 \left( I_{wheel} + I_{low-speed coupling} + I_{gearbox} + R^2 \left( I_{high-speed coupling} + I_{motor} \right) \right)
$$

I would treat the mass similarly.  The gearbox housing, motor stator and outer race of the bearing move with the pendulum, so their masses should add to $m$, while the wheels, all four couplings, gearbox internals and motor rotors should add to $M$.  In practice, I've never seen the mass of the gearbox internals on a datasheet, and I doubt that it's significant anyway.  But it is likely that the mass and inertia of the motors and at least the high-speed couplings are important, if not also the low-speed couplings.  For bearings, I usually assign half of the mass and inertia to each body and assume that it's close enough.

It's also worth mentioning that we have kind of a funny case where both the gearbox housing and the output shaft can be moved while keeping the other stationary.  The motor will rotate in response to both of these motions.  I haven't put much thought into the "correct" way to account for this effect (possibly adding a small additional term to the pendulum inertia?), but with my engineering hat on, I would feel comfortable neglecting this effect and assuming the controller will compensate anyway.


问:
How can I calculate processing speed of microcontroller
I need a microcontroller that can process minimum 2mb data per second.
How do I determine what processors will be able to do this?

Also how can I calculate the processing speed in per second of any microcontroller?

I am very much scared with my college project and I need help.

答:
You need to consult the specifications. There is absolutely no other way unless you buy the micro, code it and run tests.

So first, you need to define what exactly processing means. Best way to do it is to write the program that does the processing, in assembly. Let's take a hypothetical program in a hypothetical assembly:

```<code>load R1, portA
add R1, R1, 1
store portB, R1
</code>```

First thing you need to know is how many cycles it takes the microcontroller to execute this code. Again, you need to consult the data sheet of the micro. Hypothetical numbers:

Load takes 3 cycles

Load from PortA adds 2 cycles overhead

Add takes 1 cycle

Store takes 2 cycles

Store to PortB adds 4 cycles overhead

So you would know that for each piece of data you read (hypothetically, 1 byte) you need 12 cycles to process them, assuming the micro is stupid enough to not use pipelines, prefetching of any sort etc.

If you read the specifications well, you will now get a good estimate of the speed of the micro for your task. In our hypothetical example, 1 byte per 12 cycles.

So to address your first question, how to know if the micro is good enough for your task, it's quite simple. You want X bytes of data per second so it takes 12X cycles to do that. All you need to know is how many cycles per second the micro runs (let's say Y). So if Y > 12X you're good.

Your second question is also simple to answer. You already have the speed in terms of "per cycle". To calculate the speed in terms of "per second", you just multiply the per-cycle speed by cycles/second of the microcontroller (again, found in the data sheet). In our hypothetical example:

```<code>speed (byte/cycle) = 1/12
speed (byte/second) = 1/12 * Y
</code>```

Once you define your task, so it would be clear how heavy the computation is, probably higher grade students or even vendors would be able to help you pick up a suitable micro.

ContentLicense: CC BY-SA 3.0

Body:
You need to consult the specifications. There is absolutely no other way unless you buy the micro, code it and run tests.

So first, you need to define what exactly processing means. Best way to do it is to write the program that does the processing, in assembly. Let's take a hypothetical program in a hypothetical assembly:

load R1, portA
add R1, R1, 1
store portB, R1

First thing you need to know is how many cycles it takes the microcontroller to execute this code. Again, you need to consult the data sheet of the micro. Hypothetical numbers:

Load takes 3 cycles

Load from PortA adds 2 cycles overhead

Add takes 1 cycle

Store takes 2 cycles

Store to PortB adds 4 cycles overhead

So you would know that for each piece of data you read (hypothetically, 1 byte) you need 12 cycles to process them, assuming the micro is stupid enough to not use pipelines, prefetching of any sort etc.

If you read the specifications well, you will now get a good estimate of the speed of the micro for your task. In our hypothetical example, 1 byte per 12 cycles.

So to address your first question, how to know if the micro is good enough for your task, it's quite simple. You want X bytes of data per second so it takes 12X cycles to do that. All you need to know is how many cycles per second the micro runs (let's say Y). So if Y > 12X you're good.

Your second question is also simple to answer. You already have the speed in terms of "per cycle". To calculate the speed in terms of "per second", you just multiply the per-cycle speed by cycles/second of the microcontroller (again, found in the data sheet). In our hypothetical example:

speed (byte/cycle) = 1/12
speed (byte/second) = 1/12 * Y

Once you define your task, so it would be clear how heavy the computation is, probably higher grade students or even vendors would be able to help you pick up a suitable micro.


问:
A sensor that can see glass/transparent objects and surfaces
I am currently thinking about making a robot that will autonomously drive around the place. The place I want this robot to drive in however contains quite a few glass walls. When mapping the area I would need to be able to see the glass. For this reason I am in need of a sensor that can see the glass, and not see through it. What kind of sensor would be the best for me? I need it to have a maximum range of about 2-10 meters and a minimum range of about 0.25 meters (preferably as small as possible). I was thinking about maybe using ultrasonic, but I was told that a laser-based sensor would probably be best. I could however only find industrial grade laser sensors that could see glass/transparent objects.</p>

答: Unless you need very good resolution or a very tight detection pattern, I would go with an ultrasonic sensor. They can be very cheap and easily meet your detection range specs, for the very low end look at something like the HC-SR04, and for more serious applications Maxbotix offers a wide range of sensors.</p>


问:
IMU in a gimbal system
I have short question about the alexmos gimbal controller. The Controller receives the gyroscope and accelerometer sensor data from the IMU, that is mounted on the camera. In optimal case, the camera should stay in perfect position, which means, that there would be no gyroscope data, since there is no movement. So the only data for positioning would be the accelerometer. Is there a second IMU onboard that receives the gyrometer data?

All the gimbal controllers from aliexpress seems not to have an IMU onboard, but it that case the controller can only use the accelerometer, right?

答: The alexmos and other similar gimbal controllers, are using the camera mounted accelerometer and gyroscope in order to identify the angle and angular rate (and possibly higher order dynamics) of the camera platform, in an attempt to keep the angle the same as the set point. You are right that if the platform was doing it's job perfectly that the sensors would "see" no change, but this is not the case. The platform moves constantly but the sensors are able to sense very small movements very quickly much smaller than could be picked up by the cameras themselves and correct before they become errors that could be see in the video. Some controllers take input from the flight controller or gyros onboard the craft in order to predict or lead movements, in order to make the response to stimuli faster, and enable proactive correction as opposed to just reactive movement.</p>

Title: IMU in a gimbal system


问:
Robot never goes straight
I am using 2 identical DC motors and a castor wheel. The motors are connected to L293D motor driver and are controlled by RPi.

The robot is not going straight. It veers off to the right. I am running both the motors at 100% PWM.

What I tried to correct the error:

I adjusted the PWM of the wheel going faster to 99%, but the robot just turns to the other side;

I adjusted the weight on the robot and the problem still persists.

I once tried to run the motor without any load. Is that the cause of this, as I was later told that, running a DC motor without any load damages them?

If that is not the cause, then please tell me how to solve this problem without using any sensors for controlling it.

答: Wheels with dc motors should b on front. And coster wheel at back. Then it will move straight only in forward direction</p>

Title: Robot never goes straight


问:
Is the working principle of all servo motors the same?
If I can control a small hobby servo motor then can I control high torque servo motor in same way? I want to make a robot which needs a high torque and speed motors. But first before buying some industrial servo motors I want to first test the kinematics of my robot with small cheap servo motors. I will be using ros-orocos toolchain to control the robot and to make an efficient motion planning algorithm. Why I can't go for expensive servo motors right now is that first I want to test the working of the robot arm, whether it moves as expected or not. Although I think it is possible, I want to be sure.</p>

答:
No, industrial servo motors are not generally controlled in the same way as hobby servos.

The servo motor, motor drive and position sensors which are combined in a hobby servo are separate items in industrial systems, and the control system is responsible for position feedback. Similar to hobby servos, the motor drive may take a PWM signal or a digital signal, but that signal will usually be the desired drive speed. The motor drive will then attempt to drive the motor at that speed. Look around a supplier's site such as Emerson or Schneider for examples of servo motors and drives.

If you want position control, or force control, then you need to put your own sensor in a feedback loop and request the desired motor speed, rather than requesting a position as you do in most hobby servos - somewhat similar to continuous rotation hobby servos.


问:
How to measure pull force on a (kite) rope?
I'm building a kite flying robot, in which I've already got some sensors built in. I now also want to measure the pulling force on the rope which is attached to the kite. So I thought of attaching a hanging digital scale to the rope which can then measure the pulling force in kilograms. I of course need to read out the data from the scale using GPIO pins or USB.

Furthermore, I also have a raspberry pi installed within the kite and I want to measure some pull on ropes in the air. So if possible, I also need very small/light scales from which I can read out the data. It would be great if the scales can measure up to about 50kg, but with some pulleys about 20kg would be fine as well.

Unfortunately I can't find any digital hanging scale with a possibility to read out the data and which is reasonably light. There are some simple USB powered hanging scales, but I think they just use USB for charging, not for reading out data. And I also found this one, but that's a bit overkill (too heavy and too expensive).

So question #1: Does anybody know where I can get a simple existing hanging scale from which I can read out data?

If needed I can also build something, but I just wouldn't know where to start. I did find this page on Alibaba, where they show the contents of the scale they offer:

So as far as I understand I need the component which I highlighted. But I have no idea how that component is called (what do I search for?) and whether it is actually doable to read it out from a Raspberry Pi.

Question #2: does anybody know what the highlighted component is called, where I could possibly get it and if I can read it out from a Raspberry Pi?

In conclusion; can anybody point me in the right direction?

答:
The component you highlighted is called a tensile load cell. You could buy one from a supplier, but it probably would be cheaper to buy a cheap digital hanging scale and taking the sensor out of it - at least I couldn't find one less than $100, ten times the price of a 50kg digital hanging scale.

You will need to do some analog conversion and amplification of the signal and then convert to digital with an ADC; I haven't done so myself but Sparkfun's load cell tutorial seems a good start.

The other means is to create your own scale, with a stiff spring and some sensor detecting its movement, such as a digital depth gauge - which costs $3 and has a serial output.


问:
How to mock a device during development?
I am coding a little robot controlled by a raspberry pi zero.
Disclaimer: This is a more general development question, and only indirectly related to raspberry pi.

Background: I don't want to test everything on the robot directly, this is too time consuming. Therefore I am trying to implement mock interfaces. For example, I'd am using a set of fake sensor readings and feed them to the class responsible for sensing and driving. Running this code in command line should for example print out "driving left" instead of starting the right motor and so on.

Question: How to mock a device so that I can use the main code without modification on my robot.

Code examples
I have currently the following file structure:

main.py: running the loop so the robot is doing something and       currently holding mock sensor data.

acc.py: AutonomousCruiseControl class: taking care of the process of       measuring and steering

hcsr04.py: a class mocking the sonar    distance reader device

servo.py: currently doing nothing: Should    contain a mock device writing to console what motor is turned  into    what direction

In the following code examples, you can see e.g. in the drive() method, that I have code purely for testing mixed with the code destined for production.

main.py

```<code>#!/usr/bin/env python3
# coding=UTF-8

from navigation import AutonomousCruiseControl

def main():
    #self.objectColisionRange = [10,15,20,15,10]
    readings = [
        [11, 16, 21, 16, 11],       # path free
        [100, 100, 19, 100, 100],   # front blocked
        [9, 100,20, 100, 100],      # center & left blocked
        [100, 100, 10, 100, 10],    # center & right blocked
        [10, 100, 10, 100, 10],     # center, left & right blocked
        [100, 10, 100, 100, 100],   # center free, left front object to avoid
        [100, 100, 100, 10, 100],   # center free, right front object to avoid
        [100, 10, 100, 10, 100]     # center free, but left and right (45°) objects too narrow
    ]

    ACC = AutonomousCruiseControl()
    # range second parameter is numbers to generate not the right boundary
    for i in range(0,8):
        ACC.front_sonar.readings = readings[i]
        print("{}. reading: {}".format(i, ACC.read_front_sonar()))
        print("{}. FOS: {}".format(i, ACC.get_front_object_status()))
        ACC.drive()

if __name__ == '__main__':
    main()
</code>```

acc.py

```<code>#!/usr/bin/env python3
# coding=UTF-8

from navigation.hcsr04 import Hcsr04
from navigation.servo import Servo
from random import randint

class AutonomousCruiseControl:
    """helping to steer a robot trough obstacles"""

    def __init__(self, object_colision_range=None, front_sonar_angles=None):
        # initiate sensors
        self.front_sonar = Hcsr04()
        self.front_sonar_servo = Servo()
        self.collision_distance = 20

        if object_colision_range is None:
            self.object_colision_range = [10, 15, 20, 15, 10]
        else:
            self.object_colision_range = object_colision_range

        if front_sonar_angles is None:
            self.front_sonar_angles = [0, 45, 90, 135, 180]
        else:
            self.front_sonar_angles = front_sonar_angles

        self.front_sonar_distances = [200, 200, 200, 200, 200]
        self.read_front_sonar()
        print(self.front_sonar_distances)

        self.front_object_status = [0, 0, 0, 0, 0]

    def read_front_sonar(self):
        """ Setting the angle and calling the read_distance method. """
        # TODO implement reading from right to left and left to right, so both servo swings can be used
        # go through all angles and read distance put it into front_sonar_distances
        # initiate only once for test cases, to not mix values
        for i in range(0, len(self.front_sonar_angles)):
            # set servo to angle
            angle = self.front_sonar_angles[i]
            # read distance
            distance = self.read_distance(self.front_sonar, angle)
            # print("distance is {}\n".format(distance))
            self.front_sonar_distances[i] = distance
        return self.front_sonar_distances

    def get_front_object_status(self):
        """ Creating a simplified form of the distance measures. Currently if a reading is equal or below the
            reading angle specific collision range, the status is set to 1. Zero means, no obstacle. """
        for i in range(0, len(self.front_sonar_distances)):
            if self.front_sonar_distances[i] <= self.object_colision_range[i]:
                self.front_object_status[i] = 1
            else:
                self.front_object_status[i] = 0
        return self.front_object_status

    def read_distance(self, sensor, angle):
        """ Remove angle later from this method and from hcsr04. """
        if sensor is self.front_sonar:
            # TODO: implement servo, set angle
            # print("Set servo to angle {} °".format(angle))
            # angle/index won't be necessary after the real servo routine is working
            # this just helps the dry run tests
            index = self.front_sonar_angles.index(angle)
            return self.front_sonar.get_distance(index)

    def drive(self):
        """ Contolling the motors with obstacle avoidance switch.
            Light search is not yet implemented. The robot should aimlessly
            cruise around without hitting detectable obstacles. """

        stat = self.front_object_status
        dist = self.front_sonar_distances

        if stat[1] + stat[2] + stat[3] is 0:
            # road is free
            print("Road free: L(eft) & R(ight) forward")
        elif stat[1] is 1:
            # left blocked
            print("Obstacle left: L(eft) faster  & R(ight) slower forward")
        elif stat[3] is 1:
            # right blocked
            print("Obstacle right: L(eft) slower  & R(ight) faster forward")
        elif stat[2] is 1 or (stat[1] + stat[3] is 2):
            # front blocked
            print("obstacle front: stop all")
            dist_sum_left = dist[0] + dist[1]
            dist_sum_right = dist[3] + dist[4]

            if dist_sum_left == dist_sum_right:
                # both sides are free and distance the same
                # choose randomly left and right 0 is left and 1 is right
                rand = randint(0, 1)
                if rand is 0:   # left
                    print("Random turn left, L backw, R forw")
                else:           # right
                    print("Random turn right, R backw, L forw")
            elif dist_sum_left > dist_sum_right:
                # left has more leeway
                print("Left leeway, turn left")
            elif dist_sum_left < dist_sum_right:
                # right has more leeway
                print("Right leeway, turn right")
</code>```

hcsr04.py

```<code>#!/usr/bin/env python3
# coding=UTF-8

# Test method

class Hcsr04:
    'this just fakes the reading from sonar it needs the servo angle where the reading should come from'
    def __init__(self):
        self.readings = [88, 88, 88, 88, 88]

    def get_distance(self, index):
        return self.readings[index]
</code>```

答:
The general name for what you're looking for is dependency injection

The pattern is to ensure that the sensors ("the dependencies") are provided to the main control system at configuration time, instead of being hardcoded deep-down. You can then have two toplevel paths. One creates the objects that connect to the actual devices and passes them when creating the main control system. The other one creates the mock objects.

Generally speaking, if you start having these kind of issues, consider using the frameworks that already exist (ROS, Rock, ...).


问:
Optimal location of the center of mass for an inverted pendulum
I'm building an inverted pendulum to be controlled by DC motors, but I've run across a conundrum.  Personal life experience tells me that it's better to have a lower center of mass to maintain balance.  On the other hand, the greater the moment of inertia (e.g. the higher the center of mass), the easier it is to maintain balance as well.

These two views both seem plausible, and yet also seem contradictory.   For an inverted pendulum, is there an optimal balance between the two perspectives?  Or is one absolutely right while the other absolutely wrong?  If one is wrong, then where is the error in my thinking?

答:
Since your control input to the system acts only on the cart, then you can influence the pendulum angle only due to the coupling between the cart motion and the pendulum motion.  If you were to put the pendulum CG at the pivot, there is no coupling between the cart and the pendulum, and the pendulum angle is no longer controllable.  This can be verified by checking the system's controllability matrix.

On paper, having a non-zero $l$ is good enough - the system is controllable as soon as the CG is not on the pivot axis.  In practice, however, you need to be able to control the pendulum angle when a reasonable amount of force is applied to the cart (what is reasonable depends on your preferences and budget).  When the pendulum is vertical ($\theta$ = 0), the pendulum acceleration is proportional to the cart acceleration:

$$
\ddot{\theta} I = -\ddot{x} m l
$$

If your goal is just to stabilize the pendulum, it may be desirable to have a large $m l$ and a small $I$ - so your control inputs more directly affect the pendulum angle.  If you have constraints on force (and power), cart displacement, etc. or if you have other goals then you may have to choose these parameters differently.

As pointed out in other answers, some systems are more stable when the CG is lower.  An inverted pendulum, however, is unstable (or for $\theta = 0$, marginally stable) for all (non-zero) values of $l$, so this theory doesn't apply.  In general, it is desirable to have a large $l$ and small $I$ because it makes it easier to control the pendulum.


问:
Mathematical modelling of system dynamic on matlab
I want to make a matlab (simulink) control model for the system in the image below.

The original pdf is only accessible if logged in Carleton's Learning Management System.

How do I get the dynamics of the system with given details in the image?

答: A very broad but a valid question. Simulink and the Simscape Addon are used both as a physical simulation toolkit and as an environment for controller development. On the concept drawing in the OP a marsrover is seen which probably should move around and make some tricks. The first thing is to create a simulation. Thats like a computergame where Simulink works as a physics engine. One step further is to develop a controller on top of the simulation. In most cases this is done with a Domain Specific Language. An example for the "Differential Drive implementation" is given on A Domain Specific Language for Modeling Differential Constraints of Mobile Robots, page 4 That paper is not Simulink specific but gives a more general idea.</p>


问:
DX100 controller communication protocol
I have a motoman robot for use in a pick and place application. It has a DX100 controller which has an ethernet interface which could be used to control a slave device using the Modbus TCP protocol.

The DX100 controller also supports Ethernet/IP and DeviceNet.
I know Modbus can be quite complex for first timers and I have little experience when it comes to programming these devices.

I would like to know, if someone here has ever worked with this controller, which communication protocol they used and why.

答:
I did not work with Motoman controllers, but I did work with other robots, Modbus and DeviceNet.

Modbus TCP is probably the easiest to use from the client side. On the DX100 Side you need to check which modbus address coresponds to the functionalites you would prefer. If you want to move the robot along the X axis, you need to check in the documentation which Modbus register(s) you need to write to make the robot move. (Please note that since I do not know the Motoman controller it is not sure that moving the robot via modbus is possible.)

After you have identified the needed registers, you can connect a freely available modbus slave (or master, depending on configuration) running on a normal PC to see if the robot moves when you write the registers. You can find a number of programs capable of doing this here.

I assume it is needed that the robot is not connected to a PC but to a PLC via Modbus. If you have correctly identified the required registers via the PC software, you can cofigure the PLC to do the same, read/write the required Modbus registers and you are done.

Title: DX100 controller communication protocol


答: I'm pretty sure that a very basic IR proximity sensor would do the trick. Glass is opaque to all but visible light. IR (as well as UV) will not penetrate the glass and you ought to be getting reliable distance measurements to the glass's surface.</p>

Title: A sensor that can see glass/transparent objects and surfaces

Body: I am currently thinking about making a robot that will autonomously drive around the place. The place I want this robot to drive in however contains quite a few glass walls. When mapping the area I would need to be able to see the glass. For this reason I am in need of a sensor that can see the glass, and not see through it. What kind of sensor would be the best for me? I need it to have a maximum range of about 2-10 meters and a minimum range of about 0.25 meters (preferably as small as possible). I was thinking about maybe using ultrasonic, but I was told that a laser-based sensor would probably be best. I could however only find industrial grade laser sensors that could see glass/transparent objects.</p>


答:
the Simulink diagram is straightforward. It is a matter of connecting blocks. For the differential equations provided in your post, the simulink is

For $u_1$ and $u_2$, I've chosen the unit step. You can change that of course. For $L$, I've set it to 0.5 since you didn't provide the actual value. The result of the position of the vehicle is shown below:

Title: Mathematical modelling of system dynamic on matlab

Body:
I want to make a matlab (simulink) control model for the system in the image below.

The original pdf is only accessible if logged in Carleton's Learning Management System.

How do I get the dynamics of the system with given details in the image?


答:
What about a flex sensor? Here's one for less than $10 at Sparkfun.

Wherever there is tension there must also be flexsion in a crossmember of the kite or in the membrane. A little bit of experimentation might get you the data you seek.

Title: How to measure pull force on a (kite) rope?

Body:
I'm building a kite flying robot, in which I've already got some sensors built in. I now also want to measure the pulling force on the rope which is attached to the kite. So I thought of attaching a hanging digital scale to the rope which can then measure the pulling force in kilograms. I of course need to read out the data from the scale using GPIO pins or USB.

Furthermore, I also have a raspberry pi installed within the kite and I want to measure some pull on ropes in the air. So if possible, I also need very small/light scales from which I can read out the data. It would be great if the scales can measure up to about 50kg, but with some pulleys about 20kg would be fine as well.

Unfortunately I can't find any digital hanging scale with a possibility to read out the data and which is reasonably light. There are some simple USB powered hanging scales, but I think they just use USB for charging, not for reading out data. And I also found this one, but that's a bit overkill (too heavy and too expensive).

So question #1: Does anybody know where I can get a simple existing hanging scale from which I can read out data?

If needed I can also build something, but I just wouldn't know where to start. I did find this page on Alibaba, where they show the contents of the scale they offer:

So as far as I understand I need the component which I highlighted. But I have no idea how that component is called (what do I search for?) and whether it is actually doable to read it out from a Raspberry Pi.

Question #2: does anybody know what the highlighted component is called, where I could possibly get it and if I can read it out from a Raspberry Pi?

In conclusion; can anybody point me in the right direction?


问:
serial monitor not showing proper output
I was reading the encoder value in arduino uno. But the output is not comming properly. It is coming like this:

I am using this arduino code to read encoder:

```<code>/* Rotary encoder read example */
#define ENC_A 14
#define ENC_B 15
#define ENC_PORT PINC

void setup()
{
  /* Setup encoder pins as inputs */
  pinMode(ENC_A, INPUT);
  digitalWrite(ENC_A, HIGH);
  pinMode(ENC_B, INPUT);
  digitalWrite(ENC_B, HIGH);
  Serial.begin (115200);
  Serial.println("Start");
}

void loop()
{
 static uint8_t counter = 0;      //this variable will be changed by encoder input
 int8_t tmpdata;
 /**/
  tmpdata = read_encoder();
  if( tmpdata ) {
    Serial.print("Counter value: ");
    Serial.print(counter, DEC);
    counter += tmpdata;
  }
}

/* returns change in encoder state (-1,0,1) */
int8_t read_encoder()
{
  static int8_t enc_states[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
  static uint8_t old_AB = 0;
  /**/
  old_AB <<= 2;                   //remember previous state
  old_AB |= ( ENC_PORT & 0x03 );  //add current state
  return ( enc_states[( old_AB & 0x0f )]);
}
</code>```

答: Looks line your terminal has the wrong baud rate, in your code you set it to 115200 baud with the line Serial.begin (115200); You can change th baud rate on your serial terminal by using the highlighted drop down menu:


问:
Estimate Disparity Error for Depth Accuracy Estimation
I would like to get a rough estimate of the depth accuracy / uncertainty of a stereo camera system. For this I would like to use the basic formula in the attached image. What is still unclear to me is what's a reasonable choice for the disparity error $\Delta$D. Unfortunately, for example this answer (in point "Resolution") describes the assumption of the size of the disparity error only very quickly.

Is a reasonable assumption just the width of one pixel? Why yes, why not?

答: The answer is that the Disparity error depends on the accuracy of the matching algorithm which matches the points in the two images. Some of these algorithms have an accuracy of one pixel, some achieve even sub-pixel accuracy.</p>

ContentLicense: CC BY-SA 4.0


问:
Is there an online RobotC IDE?
I'm on a Linux chromebook, and am using a Vex robot, and am wondering if there is an online IDE for RobotC that will run my computer?</p>

答:
Unfortunately, there is no RobotC version you can run on your computer as configured - RobotC is currently (as of March 2017) only available for Windows.

Depending on your Chromebook, there may be a way to run Windows software - check out this guide:

https://www.howtogeek.com/173353/how-to-run-windows-software-on-a-chromebook/

Title: Is there an online RobotC IDE?


问:
Robotc color sensor error
I am trying to write a simple program where the robot(Lego NXT2) will follow a blue line.

```<code>#pragma config(Sensor, S1,     ,               sensorCOLORBLUE)
//!!Code automatically generated by 'ROBOTC' configuration wizard               !!//

task main()
{
    while(true)
    {
        if(SensorValue[S1] == sensorCOLORBLUE)
        {
            motor[motorB] = 0;
            motor[motorC] = -50;
        }
        else
        {
            motor[motorB] = -50;
            motor[motorC] = 0;
        }
    }
    wait1Msec(1);
}
</code>```

I am using an nxt color sensor and the problem is that only 1 motor is moving. I know that none of the motors are broken either because I tested them out.
Can somebody help me diagnose my problem?

答:
Your comparison is wrong - sensorColorBLUE is a sensor setting, not a color. To detect a color, you need your sensor in RGB mode, and your comparison needs to be against a TColors type. Change the code as follows:

```<code>#pragma config(Sensor, S1,     colorSensor,    sensorColorNxtFULL)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
    while(true)
    {
        if((TColors)SensorValue[colorSensor] == BLUECOLOR)
        {
            motor[motorB] = 0;
            motor[motorC] = -50;
        }
        else
        {
            motor[motorB] = -50;
            motor[motorC] = 0;
        }
    }
    wait1Msec(1);
}
</code>```

I kept the rest of your code [almost] as-is, however I suggest naming the sensor, as shown in my code. I also suggest naming your motors in "Motors and Sensors setup"; if you're applying -50 power because the motor is reversed, you should reverse the motors in setup.

Finally, you don't need the wait1Msec(1) command - that doesn't do anything at all here - the NXT color sensor will take about 3ms to scan, this additional delay is not at all necessary.

Title: Robotc color sensor error

Body:
Your comparison is wrong - sensorColorBLUE is a sensor setting, not a color. To detect a color, you need your sensor in RGB mode, and your comparison needs to be against a TColors type. Change the code as follows:

#pragma config(Sensor, S1,     colorSensor,    sensorColorNxtFULL)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
    while(true)
    {
        if((TColors)SensorValue[colorSensor] == BLUECOLOR)
        {
            motor[motorB] = 0;
            motor[motorC] = -50;
        }
        else
        {
            motor[motorB] = -50;
            motor[motorC] = 0;
        }
    }
    wait1Msec(1);
}

I kept the rest of your code [almost] as-is, however I suggest naming the sensor, as shown in my code. I also suggest naming your motors in "Motors and Sensors setup"; if you're applying -50 power because the motor is reversed, you should reverse the motors in setup.

Finally, you don't need the wait1Msec(1) command - that doesn't do anything at all here - the NXT color sensor will take about 3ms to scan, this additional delay is not at all necessary.


问:
Help with setting up Inverse Kinematics
I'm working through the Inverse Kinematic example for the Unimation PUMA 560  from Introduction to Robotics by Craig. In it he specifies the IK equations like so:

In my software program I have three sliders on the screen that will give me the rotation of the end point in x, y, z like so (this is in Unity):

Each one of these sliders will control a float variable in the code (C#) and I can read these into my script (Using Unity 5). I am trying to replicate the inverse kinematic solution for this PUMA robot inside Unity, so that for a given position and rotation of the end effector the link rotations will update accordingly. I have already written out the IK equations that Craig specified in the example to calculate theta(i), but how do I "read" the slider values and "input" them to these equations? If I am not making any sense I apologize, I have been chipping away at this for some time and hit a mental blank wall. Any advice appreciated.

Edit: So in my near-delirious state I have not posited my question properly. So far, these are the equations I have written so far in code:

public class PUMA_IK : MonoBehaviour {

```<code>GameObject J1, J2, J3, J4, J5, J6;

public Vector3 J2J3_diff, J3J4_diff;

public Slider px_Slider;
public Slider py_Slider;
public Slider pz_Slider;
public Slider rx_Slider;
public Slider ry_Slider;
public Slider rz_Slider;

public float Posx, Posy, Posz, Rotx, Roty, Rotz;

float a1, a2, a3, a4, a5, a6;   //Joint twist
float r1, r2, r3, r4, r5, r6;   //Mutual perpendicular length
float d1, d2, d3, d4, d5, d6;   //Link offset
public float t1, t2, t23, t3, t4, t5, t6;   //Joint angle of rotation

public float J1Rot, J2Rot, J3Rot, J4Rot, J5Rot, J6Rot;

float r11, r21, r31, r12, r22, r32, r13, r23, r33, c23, s23, Px, Py, Pz, phi, rho, K;

int pose;   //1 - left hand, 2 = right hand

// Use this for initialization
void Start ()
{
    pose = 1;

    J1 = GameObject.FindGameObjectWithTag("J1");
    J2 = GameObject.FindGameObjectWithTag("J2");
    J3 = GameObject.FindGameObjectWithTag("J3");
    J4 = GameObject.FindGameObjectWithTag("J4");
    J5 = GameObject.FindGameObjectWithTag("J5");
    J6 = GameObject.FindGameObjectWithTag("J6");

    J2J3_diff = J3.transform.position - J2.transform.position;
    J3J4_diff = J4.transform.position - J3.transform.position;

    //Init modified DH parameters

    //Joint twist

    a1 = 0;
    a2 = -90;
    a3 = 0;
    a4 = -90;
    a5 = 90;
    a6 = -90;

    //Link length

    r1 = 0;
    r2 = Mathf.Abs(J2J3_diff.x);
    r3 = Mathf.Abs(J3J4_diff.x);
    r4 = 0;
    r5 = 0;
    r6 = 0;

    //Link offset

    d1 = 0;
    d2 = 0;
    d3 = Mathf.Abs(J2J3_diff.z);
    d4 = Vector3.Distance(J4.transform.position, J3.transform.position);
    d5 = 0;
    d6 = 0;

}

void Update ()
{
    Posx = px_Slider.value;
    Posy = py_Slider.value;
    Posz = pz_Slider.value;

    Rotx = rx_Slider.value;
    Roty = ry_Slider.value;
    Rotz = rz_Slider.value;

    Px = Posx;
    Py = Posy;
    Pz = Posz;

    c23 = ((cos(t2)*cos(t3)) - (sin(t2)*sin(t3)));
    s23 = ((cos(t2)*sin(t3)) + (sin(t2)*cos(t3)));

    rho = Mathf.Sqrt(Mathf.Pow(Px, 2) + Mathf.Pow(Py, 2));
    phi = Mathf.Atan2(Py, Px);

    if (pose == 1)
    {
        t1 = Mathf.Atan2(Py, Px) - Mathf.Atan2(d3, Mathf.Sqrt(Mathf.Pow(Px, 2) + Mathf.Pow(Py, 2) - Mathf.Pow(d3, 2)));
    }

    if (pose == 2)
    {
        t1 = Mathf.Atan2(Py, Px) - Mathf.Atan2(d3, -Mathf.Sqrt(Mathf.Pow(Px, 2) + Mathf.Pow(Py, 2) - Mathf.Pow(d3, 2)));
    }

    K = (Mathf.Pow(Px, 2)+ Mathf.Pow(Py, 2) + Mathf.Pow(Px, 2) - Mathf.Pow(a2, 2) - Mathf.Pow(a3, 2) - Mathf.Pow(d3, 2) - Mathf.Pow(d4, 2)) / (2 * a2);

    if (pose == 1)
    {
        t3 = Mathf.Atan2(a3, d4) - Mathf.Atan2(K, Mathf.Sqrt(Mathf.Pow(a2, 2) + Mathf.Pow(d4, 2) - Mathf.Pow(K, 2)));
    }

    if (pose == 2)
    {
        t3 = Mathf.Atan2(a3, d4) - Mathf.Atan2(K, -Mathf.Sqrt(Mathf.Pow(a2, 2) + Mathf.Pow(d4, 2) - Mathf.Pow(K, 2)));
    }

    t23 = Mathf.Atan2(((-a3 - (a2 * cos(t3))) * Pz) - ((cos(t1) * Px) + (sin(t1) * Py)) * (d4 - (a2 * sin(t3))), ((((a2 * sin(t3)) - a4) * Pz) - ((a3 + (a2 * cos(t3))) * ((cos(t1) * Px) + (sin(t1) * Py)))));

    t2 = t23 - t3;

    if (sin(t5) != 0)   //Joint 5 is at zero i.e. pointing straight out
    {
        t4 = Mathf.Atan2((-r13 * sin(t1)) + (r23 * cos(t1)), (-r13 * cos(t1) * c23) + (r33 * s23));
    }

    float t4_detection_window = 0.00001f;

    if ((((-a3 - (a2 * cos(t3))) * Pz) - ((cos(t1) * Px) + (sin(t1) * Py)) < t4_detection_window) && (((-r13 * cos(t1) * c23) + (r33 * s23)) < t4_detection_window))
    {
        t4 = J4Rot;
    }

    float t5_s5, t5_c5; //Eqn 4.79

    t5_s5 = -((r13 * ((cos(t1) * c23 * cos(t4)) + (sin(t1) * sin(t4)))) + (r23 * ((sin(t1) * c23 * cos(t4)) - (cos(t1) * sin(t4)))) - (r33 * (s23 * cos(t4))));

    t5_c5 = (r13 * (-cos(t1) * s23)) + (r23 * (-sin(t1) * s23)) + (r33 * -c23);

    t5 = Mathf.Atan2(t5_s5, t5_c5);

    float t5_s6, t5_c6; //Eqn 4.82

    t5_s6 = ((-r11 * ((cos(t1) * c23 * sin(t4)) - (sin(t1) * cos(t4)))) - (r21 * ((sin(t1) * c23 * sin(t4)) + (cos(t1) * cos(t4)))) + (r31 * (s23 * sin(t4))));

    t5_c6 = (r11 * ((((cos(t1) * c23 * cos(t4)) + (sin(t1) * sin(t4))) * cos(t5)) - (cos(t1) * s23 * sin(t5)))) + (r21 * ((((sin(t1) * c23 * cos(t4)) + (cos(t1) * sin(t4))) * cos(t5)) - (sin(t1) * s23 * sin(t5)))) - (r31 * ((s23 * cos(t4) * cos(t5)) + (c23 * sin(t5))));

    t6 = Mathf.Atan2(t5_s6, t5_c6);

    //Update current joint angle for display

    J1Rot = J1.transform.localRotation.eulerAngles.z;
    J2Rot = J2.transform.localRotation.eulerAngles.y;
    J3Rot = J3.transform.localRotation.eulerAngles.y;
    J4Rot = J4.transform.localRotation.eulerAngles.z;
    J5Rot = J5.transform.localRotation.eulerAngles.y;
    J6Rot = J6.transform.localRotation.eulerAngles.z;

}

void p(object o)
{
    Debug.Log(o);
}

float sin(float angle)
{
    return Mathf.Rad2Deg * Mathf.Sin(angle);
}
float cos(float angle)
{
    return Mathf.Rad2Deg * Mathf.Cos(angle);
}
</code>```

}

The issue is not with the mathematics of what is going on per se, I am just confused at how I interface the three values of the X, Y, and Z rotation for the sliders (which represent the desired orientation) with these equations. For the translation component it is easy, the slider values are simply equal to Px, Py and Pz in the IK equation set. But in his equations he references r11, r23, etc, which are the orientation components. I am unsure how to replace these values (r11, r12, etc) with the slider values.

Any ideas?

Edit 2: I should also say that these sliders would be for positioning the tool center point. The XYZ sliders will give the translation and the others would give the orientation, relative to the base frame. I hope this all makes sense. The goal is to be able to use these sliders in a similar fashion to how one would jog a real robot in world mode (as opposed to joint mode). I then pass these calculated angle values to the transform.rotation component of each joint in Unity.

So what I am really asking is given the three numbers that the rotation sliders produce (XRot, YRot and ZRot), how do I plug those three numbers into the IK equations?

答:
If I have correctly understood your question, you have those values (the angles that you want for the pose of the end-effector of the robot) but you don't actually know how to use them, since in the rotation matrix there are 9 parameters r i,j that look uncorrelated from the three angles.

So, the first thing that probably you have seen in Craig's book is that a rotation matrix, even though it has 9 parameters, has actually only 3 dof in space. You can represent a rotation in space with only 3 angles: these are called Euler angles, or RPY angles, depending on your choice. Euler angles are three rotations around X,Y,Z angles (there are actually 12 combinations possible, like Y,Z,X, X,Y,X and so on), but they are made around moving axes: after you do your first rotation, your reference frame will rotate accordingly, and you execute the second rotation around your new axis (be it X,Y, or Z depending on your formalization). You can have a more accurate explanation on the textbook if you wish.
Roll Pitch Yaw angles are similar, but they are around fixed angles, i.e. you do three rotations around X, Y and Z around the fixed axis from which you start.
You can search for visual examples on the web that will make you understand these concepts very clearly.
So, all this introduction is to say that you first have to model your problem: do you want these three sliders to set angles around moving or fixed angles? After you have modeled your representation, you can compute the rotation matrix from these three simple rotations around a single axis. You will end up with three simple rotation matrices around one axis, and you just multiply those three. You will end up with a new 3x3 matrix, that is your complete rotation matrix, the same that you see in that example: from that, you can extract each value of r i,j and evaluate the inverse kinematic according to your algorithm.

Title: Help with setting up Inverse Kinematics


答:
There is a very good Handbook of Robotics from Ken Waldron and Jim Schmiedeler that provides the equations you're looking for.  The book can be purchased at this Springer site, and you may be able to search to find other copies. As @dante mentions, the mapping of the 3x3 rotation matrix to wrist orientation depends on the type of angles you are using to describe the wrist orientation.

In Table 1.2 of the referenced book they show how Euler angles can be obtained from the rotation matrix:

Z-Y-X Euler Angles $[\alpha, \beta, \gamma]$ are obtainable as:
$$ \beta = {\mathbb {Atan2}}(-r_{31}, \sqrt{r^2_{11} + r^2_{21}}) $$
$$ \alpha = {\mathbb {Atan2}}(r_{21} / {\cos \beta}, r_{11} / {\cos \beta})$$
$$ \gamma = {\mathbb {Atan2}}(r_{32} / {\cos \beta}, r_{33} / {\cos \beta})$$

Similarly, the X-Y-Z Fixed Angles mapping is also given in the paper's Table 1.2:

X-Y-Z Fixed Angles $[\psi, \theta, \phi]$:

$$ \theta = {\mathbb {Atan2}}(-r_{31}, \sqrt{r^2_{11} + r^2_{21}})$$
$$ \psi = {\mathbb {Atan2}}(r_{21} / \cos \theta, r_{11} / \cos \theta)$$
$$ \phi = {\mathbb {Atan2}}(r_{32} / \cos \theta, r_{33} / \cos \theta)$$

The authors also show conversions from a couple of additional wrist rotation coordinate systems.

Title: Help with setting up Inverse Kinematics

Body:
I'm working through the Inverse Kinematic example for the Unimation PUMA 560  from Introduction to Robotics by Craig. In it he specifies the IK equations like so:

In my software program I have three sliders on the screen that will give me the rotation of the end point in x, y, z like so (this is in Unity):

Each one of these sliders will control a float variable in the code (C#) and I can read these into my script (Using Unity 5). I am trying to replicate the inverse kinematic solution for this PUMA robot inside Unity, so that for a given position and rotation of the end effector the link rotations will update accordingly. I have already written out the IK equations that Craig specified in the example to calculate theta(i), but how do I "read" the slider values and "input" them to these equations? If I am not making any sense I apologize, I have been chipping away at this for some time and hit a mental blank wall. Any advice appreciated.

Edit: So in my near-delirious state I have not posited my question properly. So far, these are the equations I have written so far in code:

public class PUMA_IK : MonoBehaviour {

```<code>GameObject J1, J2, J3, J4, J5, J6;

public Vector3 J2J3_diff, J3J4_diff;

public Slider px_Slider;
public Slider py_Slider;
public Slider pz_Slider;
public Slider rx_Slider;
public Slider ry_Slider;
public Slider rz_Slider;

public float Posx, Posy, Posz, Rotx, Roty, Rotz;

float a1, a2, a3, a4, a5, a6;   //Joint twist
float r1, r2, r3, r4, r5, r6;   //Mutual perpendicular length
float d1, d2, d3, d4, d5, d6;   //Link offset
public float t1, t2, t23, t3, t4, t5, t6;   //Joint angle of rotation

public float J1Rot, J2Rot, J3Rot, J4Rot, J5Rot, J6Rot;

float r11, r21, r31, r12, r22, r32, r13, r23, r33, c23, s23, Px, Py, Pz, phi, rho, K;

int pose;   //1 - left hand, 2 = right hand

// Use this for initialization
void Start ()
{
    pose = 1;

    J1 = GameObject.FindGameObjectWithTag("J1");
    J2 = GameObject.FindGameObjectWithTag("J2");
    J3 = GameObject.FindGameObjectWithTag("J3");
    J4 = GameObject.FindGameObjectWithTag("J4");
    J5 = GameObject.FindGameObjectWithTag("J5");
    J6 = GameObject.FindGameObjectWithTag("J6");

    J2J3_diff = J3.transform.position - J2.transform.position;
    J3J4_diff = J4.transform.position - J3.transform.position;

    //Init modified DH parameters

    //Joint twist

    a1 = 0;
    a2 = -90;
    a3 = 0;
    a4 = -90;
    a5 = 90;
    a6 = -90;

    //Link length

    r1 = 0;
    r2 = Mathf.Abs(J2J3_diff.x);
    r3 = Mathf.Abs(J3J4_diff.x);
    r4 = 0;
    r5 = 0;
    r6 = 0;

    //Link offset

    d1 = 0;
    d2 = 0;
    d3 = Mathf.Abs(J2J3_diff.z);
    d4 = Vector3.Distance(J4.transform.position, J3.transform.position);
    d5 = 0;
    d6 = 0;

}

void Update ()
{
    Posx = px_Slider.value;
    Posy = py_Slider.value;
    Posz = pz_Slider.value;

    Rotx = rx_Slider.value;
    Roty = ry_Slider.value;
    Rotz = rz_Slider.value;

    Px = Posx;
    Py = Posy;
    Pz = Posz;

    c23 = ((cos(t2)*cos(t3)) - (sin(t2)*sin(t3)));
    s23 = ((cos(t2)*sin(t3)) + (sin(t2)*cos(t3)));

    rho = Mathf.Sqrt(Mathf.Pow(Px, 2) + Mathf.Pow(Py, 2));
    phi = Mathf.Atan2(Py, Px);

    if (pose == 1)
    {
        t1 = Mathf.Atan2(Py, Px) - Mathf.Atan2(d3, Mathf.Sqrt(Mathf.Pow(Px, 2) + Mathf.Pow(Py, 2) - Mathf.Pow(d3, 2)));
    }

    if (pose == 2)
    {
        t1 = Mathf.Atan2(Py, Px) - Mathf.Atan2(d3, -Mathf.Sqrt(Mathf.Pow(Px, 2) + Mathf.Pow(Py, 2) - Mathf.Pow(d3, 2)));
    }

    K = (Mathf.Pow(Px, 2)+ Mathf.Pow(Py, 2) + Mathf.Pow(Px, 2) - Mathf.Pow(a2, 2) - Mathf.Pow(a3, 2) - Mathf.Pow(d3, 2) - Mathf.Pow(d4, 2)) / (2 * a2);

    if (pose == 1)
    {
        t3 = Mathf.Atan2(a3, d4) - Mathf.Atan2(K, Mathf.Sqrt(Mathf.Pow(a2, 2) + Mathf.Pow(d4, 2) - Mathf.Pow(K, 2)));
    }

    if (pose == 2)
    {
        t3 = Mathf.Atan2(a3, d4) - Mathf.Atan2(K, -Mathf.Sqrt(Mathf.Pow(a2, 2) + Mathf.Pow(d4, 2) - Mathf.Pow(K, 2)));
    }

    t23 = Mathf.Atan2(((-a3 - (a2 * cos(t3))) * Pz) - ((cos(t1) * Px) + (sin(t1) * Py)) * (d4 - (a2 * sin(t3))), ((((a2 * sin(t3)) - a4) * Pz) - ((a3 + (a2 * cos(t3))) * ((cos(t1) * Px) + (sin(t1) * Py)))));

    t2 = t23 - t3;

    if (sin(t5) != 0)   //Joint 5 is at zero i.e. pointing straight out
    {
        t4 = Mathf.Atan2((-r13 * sin(t1)) + (r23 * cos(t1)), (-r13 * cos(t1) * c23) + (r33 * s23));
    }

    float t4_detection_window = 0.00001f;

    if ((((-a3 - (a2 * cos(t3))) * Pz) - ((cos(t1) * Px) + (sin(t1) * Py)) < t4_detection_window) && (((-r13 * cos(t1) * c23) + (r33 * s23)) < t4_detection_window))
    {
        t4 = J4Rot;
    }

    float t5_s5, t5_c5; //Eqn 4.79

    t5_s5 = -((r13 * ((cos(t1) * c23 * cos(t4)) + (sin(t1) * sin(t4)))) + (r23 * ((sin(t1) * c23 * cos(t4)) - (cos(t1) * sin(t4)))) - (r33 * (s23 * cos(t4))));

    t5_c5 = (r13 * (-cos(t1) * s23)) + (r23 * (-sin(t1) * s23)) + (r33 * -c23);

    t5 = Mathf.Atan2(t5_s5, t5_c5);

    float t5_s6, t5_c6; //Eqn 4.82

    t5_s6 = ((-r11 * ((cos(t1) * c23 * sin(t4)) - (sin(t1) * cos(t4)))) - (r21 * ((sin(t1) * c23 * sin(t4)) + (cos(t1) * cos(t4)))) + (r31 * (s23 * sin(t4))));

    t5_c6 = (r11 * ((((cos(t1) * c23 * cos(t4)) + (sin(t1) * sin(t4))) * cos(t5)) - (cos(t1) * s23 * sin(t5)))) + (r21 * ((((sin(t1) * c23 * cos(t4)) + (cos(t1) * sin(t4))) * cos(t5)) - (sin(t1) * s23 * sin(t5)))) - (r31 * ((s23 * cos(t4) * cos(t5)) + (c23 * sin(t5))));

    t6 = Mathf.Atan2(t5_s6, t5_c6);

    //Update current joint angle for display

    J1Rot = J1.transform.localRotation.eulerAngles.z;
    J2Rot = J2.transform.localRotation.eulerAngles.y;
    J3Rot = J3.transform.localRotation.eulerAngles.y;
    J4Rot = J4.transform.localRotation.eulerAngles.z;
    J5Rot = J5.transform.localRotation.eulerAngles.y;
    J6Rot = J6.transform.localRotation.eulerAngles.z;

}

void p(object o)
{
    Debug.Log(o);
}

float sin(float angle)
{
    return Mathf.Rad2Deg * Mathf.Sin(angle);
}
float cos(float angle)
{
    return Mathf.Rad2Deg * Mathf.Cos(angle);
}
</code>```

}

The issue is not with the mathematics of what is going on per se, I am just confused at how I interface the three values of the X, Y, and Z rotation for the sliders (which represent the desired orientation) with these equations. For the translation component it is easy, the slider values are simply equal to Px, Py and Pz in the IK equation set. But in his equations he references r11, r23, etc, which are the orientation components. I am unsure how to replace these values (r11, r12, etc) with the slider values.

Any ideas?

Edit 2: I should also say that these sliders would be for positioning the tool center point. The XYZ sliders will give the translation and the others would give the orientation, relative to the base frame. I hope this all makes sense. The goal is to be able to use these sliders in a similar fashion to how one would jog a real robot in world mode (as opposed to joint mode). I then pass these calculated angle values to the transform.rotation component of each joint in Unity.

So what I am really asking is given the three numbers that the rotation sliders produce (XRot, YRot and ZRot), how do I plug those three numbers into the IK equations?


问:
How to communicate Ardupilot with Arduino
Can anyone tell me if it is possible to send data through the analogue pins of the Ardupilot to the analog pins of the Arduino?
For example, I would like to trigger a button on a channel from my radio control and Ardupilot should send a specific number to the Arduino. Would anyone have any idea how I can do this?
Thank you in advance.</p>

答: The best way to achieve this would be with RC channel pass through, you can configure any channel from 7-14 for RC passthrough (technically you can configure lower channels for this but this can cause weird things to happen as they might be sharing your pitch/roll/yaw/throttle/mode setting) You can then read in the servo value on the arduino with the pulseIn function, Ideally the servo output should be very accurate but for robustness I would have 10 µs ranges, allowing you to have over 100 unique outputs. As a bonus you can use the Do_Set_Servo command using pre programmed missions to activate your arduino at specific waypoints.</p>


答:
You need to turn each rotation angle (XRot, YRot, ZRot) into their own 3x3 rotation matrix. Once you have three 3x3 rotation matrices, you multiply them all together to get a singular "final" 3x3 rotation matrix that describes how the ending orientation is related to the starting orientation.

How you define these rotation matrices (global frame or body frame) and how you apply them (x/y/z, z/y/x, y/z/x, etc.) determines the result you'll get. For me, I found that body-centric coordinates made the most intuitive sense.

Imagine a ship whose y-axis points forward, z-axis points up, and x-axis points right (starboard). If you define roll as about the y-axis, pitch about the x-axis, and yaw about the z-axis, then (to me at least) those terms make the most sense when you say that first you yaw, then you pitch, then you roll.

So, first you yaw, using $R_z(\theta_z)$:

$$
R_z(\theta_z) = \left[
\begin{array}{ccc}
\cos{\theta_z} & -\sin{\theta_z} & 0 \\
\sin{\theta_z} & \cos{\theta_z} & 0 \\
0 & 0 & 1
\end{array} \right]
$$

Then you pitch, using $R_x(\theta_x)$

$$
R_x(\theta_x) = \left[
\begin{array}{ccc}
1 & 0 & 0 \\
0 & \cos{\theta_x} & -\sin{\theta_x} \\
0 & \sin{\theta_x} & \cos{\theta_x} \\
\end{array} \right]
$$

Then you roll, using $R_y(\theta_y)$
$$
R_y(\theta_y) = \left[
\begin{array}{ccc}
\cos{\theta_y} & 0 & \sin{\theta_y} \\
0 & 1 & 0 \\
-\sin{\theta_y} & 0 & \cos{\theta_y} \\
\end{array} \right]
$$

So, when you multiply them all together, you do matrix multiplication from right to left, you get

$$
R = R_y R_x R_z
$$

I used the y-axis as the "long" or longitudinal axis here because, at my company, that's how we set up our jobs: y-axis is generally the longer axis. I've seen online that typically the x-axis is the long axis, but it's all in how you define your axes. The $R_x$, $R_y$, and $R_z$ matrices I've given are the correct form to use for those axes, so if you redefine the axes for yourself to be something different just be sure to use the correct axis.

So, once you get your overall $R$ matrix, that's where you get your $r_11$, $r_21$, etc. values.

Title: Help with setting up Inverse Kinematics


问:
Shedding light on "cyber-physical systems"
These days, one often hears of cyber-physical systems. Reading on the subject, though, it is very unclear how those systems differ from distributed and/or embedded systems. Examples from Wikipedia itself only make them look more like traditional distributed systems. For example:

A real-world example of such a system is the Distributed Robot Garden at MIT in which a team of robots tend a garden of tomato plants. This system combines distributed sensing (each plant is equipped with a sensor node monitoring its status), navigation, manipulation and wireless networking.

Obviously, any distributed system consists of sensing, actuations (which can easily include navigation) and networking.

My question is, how exactly does cyber-physical systems differ from traditional distributed systems? Is it just a fancy name, or is there something considerably different with it?

答:
It's just a fancy (and awkward) name. Also known as robotics, cybernetics, embedded systems, control systems and so on. I'm guessing there is some dissatisfaction with either the implications or specificity of those other related categories:

Robotics - Makes people think of rovers and industrial material handling equipment.

Embedded systems - Too unspecific, doesn't necessarily have much physical interaction with the world outside of buttons and displays.

Control systems - Mostly is used to refer to deterministic and non-distributed control loops. More Z-transform, less AI.

Cybernetics - probably the closest in meaning, but over time the cyber-x has come to mean things that are purely virtual (cyberspace, cybersecurity, etc) instead of the original control system meaning. Thus the need to tack on a somewhat redundant "physical" to remind people that this involves atoms and not just bits.


答:
All other answers describe well how your problem can be answered generally, and include very useful information, but again for very generalized cases.

The point that I feel is missing, is that you are working in Unity, which has builtin solutions for most of your problems. You can reimplement euler angle calculations rotarion matrix calculations, but it will be hard to mainaint, even harder to document well.

4x4 Transformation matrices are well known and used in the paper posted by you. Here is a Unity exampe on how to uses 4x4 transformation matrices (scaling is not important for you, just use 0, 0, 0 for scaling).

Here is how Unity uses Euler angles. Here is how can quaterions be used and converted. You can also find examples on how to convert transformation matrices to rotation matrices.

Furthermore, here is an example on how to compute rotations in world coordinate systems and local coordinate systems.

The main point is, that if you have a transformation matrix in Unity you can used the builtin Unity functions the get the rotation in many possible variant, and vice-versa, if you have angles, you can get other representations (transformation matrix, quaterions) using builtin Untiy functions without the need for implementing code which is hard to test, maintain and document.

Title: Help with setting up Inverse Kinematics


问:
Why do the motors of my quadcopter not deliver sufficient power?
I am building a quadcopter using the tutorial The Ultimate PVC Quadcopter.

When ever I go to lift off, at full throttle, the motors spin but the quadcopter doesn't go anywhere. I have checked again and again and the motors are spinning in the right direction, and have the right propellers. Does anybody know why my quad won't fly?

I am using a KK2.1.5 flight controller, propellers marked with 1045r on the counter clockwise motors and the propellers just marked with 1045 on the clockwise motors.

If I switch the propellers, 1045r clockwise and 1045 counter clockwise, then my quadcopter flips over.

The layout of the motors is:

```<code>1. CW     2. CCW
4. CCW    3. CW
</code>```

I am a beginner and this is the first drone I have built/owned.

P.S. My quadcopter weighs 3.2 pounds, is using 980 kv motors, 10" propellers, 20 A ESCs, and a 3S 50C 2200 mAh Li-Po battery.

答:
If I'm understanding you correctly, you are saying that the motors spin, but they don't spin fast enough to generate enough thrust to get the drone off the ground.

It sounds to me, if this is the case, that you maybe haven't done the speed calibration for the electronic speed controllers (ESCs).

Here is the Hobbywing page on calibration, if you are using the same speed controllers used in the tutorial you linked; here is another tutorial for more generic instructions (go to the "Manual ESC-by-ESC Calibration" section).

Basically you need to be sure that the speed controller "understands" that the full throttle signal you're providing it actually means full speed.

Of course, the other obvious problem might be a dead battery.


问:
VFH+ (Vector Field Histogram+) : Is it possible to choose a candidate sector without a set goal point?
Good day

I am currently implementing the VFH algorithm.

Is it possible to configure the algorithm such that a reactionary motion is generated at the presence of an obstacle?

I have been able to generate the obstacle map, primary polar histogram and the binary polar histogram.

How does one prioritize a sector to pass through?

I have seen an implementation in labview where in it is possible to implement a simple vector field histogram path planning without any goal points here

答:
I don't know about VFH, but VFH+ has the ability to just "drive around" without a goal. A very clear explanation of VFH+ can be found here.
On page 5 they start choosing what direction to move in. I don't know how much of your code you'll be able to reuse, but I hope this helps...</p>


问:
Retrieving distance and angle of roobma create2
The goal of my project is to get a real-time location information of roobma. The information includes the x position, the y position and the angle (12 o'clock direction is 0 degree, 3 o'clock is 90 degree and so on).

I am using C++ to program the roomba.

I want to use "Stream" (Opcode: 148) to get the date, but it doesn't work for me.

This is the part I am getting stuck at:

```<code>unsigned char streamCommand[3];
streamCommand[0]=148;
streamCommand[1]=1;
streamCommand[2]=19;
write(robot,&streamCommand,3);
signed char readStream[6];

bool indicator=true;
while(indicator){
    string command="";
    cout<<"Please input your command"<<endl;
    cin>>command;
    action(command);
    read(robot,&readStream,6);
    cout<<"[0]="<<readStream[0]<<endl;
    cout<<"[1]="<<readStream[1]<<endl;
    cout<<"[2]="<<readStream[2]<<endl;
    cout<<"[3]="<<readStream[3]<<endl;
    cout<<"[4]="<<readStream[4]<<endl;
    cout<<"[5]="<<readStream[5]<<endl;
}

return 0;
</code>```

}

streamCommand is a read command to ask roomba to send back data in stream.

readStream is the array that I store my retrieve data.

Why is this code not working?

答:
You say, "It doesn't work for me." What does that mean? You get nothing? You get numbers that don't make sense? Does it lock up? Something else?

First, I'd refer you to the Create2 Documentation where I'd suggest you read up on the Opcode 148 protocol, starting on Page 20.

Notably:

Serial sequence: [148] [Number of packets] [Packet ID 1] [Packet ID 2] etc.

In your question title you say you want distance and angle (packets 19 and 20) but in your question you say you want a date (which doesn't exist). In your code though, you're only requesting packet 19, not 19 and 20. Your code should be:

```<code>streamCommand[0]=148;
streamCommand[1]=2;
streamCommand[2]=19;
streamCommand[3]=20;
</code>```

On the next page in the documentation:

The format of the data returned is:

[19][N-bytes][Packet ID 1][Packet 1 data...][Packet ID 2][Packet 2 data...][Checksum]

So, let's say for the moment that you really DO just want packet 19 right now, for troubleshooting/development purposes. In that case, the data coming back to you would look like the following:

[19][3][19][2 bytes of data][checksum]

The first byte is the header, 19.
The second byte is the number of bytes between the second byte and the checksum, which is three: the packet ID (the number 19) and the two bytes of data for that packet.
The third byte is the packet ID (19)
The fourth and fifth bytes are the data for that packet.
The sixth byte is the checksum.

The problem here that you might be having if you're getting data but it doesn't look correct is that you don't appear to ever check that you're starting in the right location; you need to parse the data you read to be sure that you're getting valid data.

Ideally, you should read bytes one at a time until you get the correct header (19). Then you should read the next byte, which is supposed to be the number of "data" bytes. Then you read however many bytes the second byte tells you to (3), then you read one more for the checksum.

Run the checksum calculation and compare your calculated checksum to the provided checksum. If the values don't match, then you either have bad data or the header you read (19) isn't the actual header but something else (maybe the packet ID - it's also 19).

In your case, you might be able to skate by knowing that you should expect [19][3][19] (or, when you get to reading distance AND angle, [19][6][19]).

If you have a three-byte sequence that goes [19][3][19], then you could probably assume that's [header][number of bytes][packet ID].

Again, though, you should really be checking the checksum, because it could be [packet ID][byte 1][byte 0], where byte 1 is 3 and byte 0 is 19. Not likely to happen, but definitely could.

Also keep in mind that the Create2 starts writing data to you every 15ms after you issue the stream command; I'm not sure how your language/hardware handles buffers but you might get a buffer overflow and/or garbage data if you don't periodically clear that input buffer.

You can handle that by reading the entire buffer, parse the data for the first (or last, or arbitrary, etc.) message, then ignoring/deleting the rest of the data, or you could enter a while/for loop to read through all the data and display all of the valid data contained in the buffer.

As a final note, again you're constantly getting data returned to you right now, but you're pausing for input in your while loop. The messages will continue to back up while you're paused, and again you don't read/clear the buffer after the pause, you only read the first six bytes.

Body:
You say, "It doesn't work for me." What does that mean? You get nothing? You get numbers that don't make sense? Does it lock up? Something else?

First, I'd refer you to the Create2 Documentation where I'd suggest you read up on the Opcode 148 protocol, starting on Page 20.

Notably:

Serial sequence: [148] [Number of packets] [Packet ID 1] [Packet ID 2] etc.

In your question title you say you want distance and angle (packets 19 and 20) but in your question you say you want a date (which doesn't exist). In your code though, you're only requesting packet 19, not 19 and 20. Your code should be:

streamCommand[0]=148;
streamCommand[1]=2;
streamCommand[2]=19;
streamCommand[3]=20;

On the next page in the documentation:

The format of the data returned is:

[19][N-bytes][Packet ID 1][Packet 1 data...][Packet ID 2][Packet 2 data...][Checksum]

So, let's say for the moment that you really DO just want packet 19 right now, for troubleshooting/development purposes. In that case, the data coming back to you would look like the following:

[19][3][19][2 bytes of data][checksum]

The first byte is the header, 19.
The second byte is the number of bytes between the second byte and the checksum, which is three: the packet ID (the number 19) and the two bytes of data for that packet.
The third byte is the packet ID (19)
The fourth and fifth bytes are the data for that packet.
The sixth byte is the checksum.

The problem here that you might be having if you're getting data but it doesn't look correct is that you don't appear to ever check that you're starting in the right location; you need to parse the data you read to be sure that you're getting valid data.

Ideally, you should read bytes one at a time until you get the correct header (19). Then you should read the next byte, which is supposed to be the number of "data" bytes. Then you read however many bytes the second byte tells you to (3), then you read one more for the checksum.

Run the checksum calculation and compare your calculated checksum to the provided checksum. If the values don't match, then you either have bad data or the header you read (19) isn't the actual header but something else (maybe the packet ID - it's also 19).

In your case, you might be able to skate by knowing that you should expect [19][3][19] (or, when you get to reading distance AND angle, [19][6][19]).

If you have a three-byte sequence that goes [19][3][19], then you could probably assume that's [header][number of bytes][packet ID].

Again, though, you should really be checking the checksum, because it could be [packet ID][byte 1][byte 0], where byte 1 is 3 and byte 0 is 19. Not likely to happen, but definitely could.

Also keep in mind that the Create2 starts writing data to you every 15ms after you issue the stream command; I'm not sure how your language/hardware handles buffers but you might get a buffer overflow and/or garbage data if you don't periodically clear that input buffer.

You can handle that by reading the entire buffer, parse the data for the first (or last, or arbitrary, etc.) message, then ignoring/deleting the rest of the data, or you could enter a while/for loop to read through all the data and display all of the valid data contained in the buffer.

As a final note, again you're constantly getting data returned to you right now, but you're pausing for input in your while loop. The messages will continue to back up while you're paused, and again you don't read/clear the buffer after the pause, you only read the first six bytes.


问:
object level sensor fusion for multiobject tracking
I want to fuse objects coming from several sensors, with different (sometimes overlapping!) fields of view. Having object lists, how can I determine whether some objects observed by different sensors are in fact the same object? Only then I can truly write an algorithm to predict future state of such an object.

From literature I read those 4 steps:

Plot to track association (first update tracks estimates and then associate by "acceptance gate" or by statistical approach PDAF or JPDAF)

Track smoothing (lots of algorithms for generating new improved estimate, e.g.: EKF, UKF, PF)

Track initiation (create new tracks from unassociated plots)

Track maintenance (delete a track if was not associated for last M turns. also: predict those tracks that were associated, their new location based on previous heading and speed)

So basically I am questioning point 1, acceptance gate. For a single sensor I can imagine it can be just a comparison of xy position of object and sensor measurement, velocity with heading eventually. My case is however, I have already ready object lists from each sensor in every cycle, there are some algorithms how to merge informations about an object collected by different sensors (great source is e.g. here: http://www.mathworks.de/matlabcentral/fileexchange/37807-measurement-fusion-state-vector-fusion), but question is how to decide which objects should be fused, and which left as they were? Fields of view may overlap partly, not totally.

答:
This depends on a lot of things.  What kind of sensors? Are they stationary? Are they at least fixed with respect to each other?

For example, if you have a system of multiple cameras that are fixed with respect to each other, you can calibrate them, so that you know their relative positions and orientations.  Then not only will you be able to determine whether two detections in different cameras are the same object, but you will be able to determine its position in 3D.

Here is the classic paper on camera calibration. To actually do the calibration you can either use OpenCV or the Caltech Camera Calibration Toolbox for Matlab. OpenCV has an example application for camera calibration which is easier to use than the Caltech toolbox.


问:
iRobot Create2+Arduino: Make the robot stop when facing an obstacle
I have been working on writing a code to control the iRobot Create movements (forward, backward, right, left and stop) from serial monitor and finally I got the correct code. I was trying to understand how to make stop moving when it face an obstacle but I couldn't get it. Also, I didn't know how to make move for a specific distance. Could anyone help me with this?

Here is my code:

```<code>#include "Arduino.h"
#include "Morse.h"
#include <SoftwareSerial.h>
#define rxPin 10
#define txPin 11
SoftwareSerial softSerial = SoftwareSerial(rxPin, txPin);
char inByte = 0; // incoming serial byte

irobot::irobot(int pin)
{
  pinMode(pin, OUTPUT);
  _pin = pin;
}

void irobot::Begin()
{
   delay(2000); // Needed to let the robot initialize
// define pin modes for software tx, rx pins:
 pinMode(rxPin, INPUT);
 pinMode(txPin, OUTPUT);
// start the the SoftwareSerial port 19200 bps (robotís default)
 softSerial.begin(19200);
// start hardware serial port
Serial.begin(19200);
softSerial.write(128); // This command starts the OI.
softSerial.write(131); // set mode to safe (see p.7 of OI manual)
delay(2000);
}

void irobot::runIt()
{
    softSerial.write(142); // requests the OI to send a packet of
    // sensor data bytes
    softSerial.write(9); // request cliff sensor value specifically
    delay(250); // poll sensor 4 times a second
    if (Serial.available() > 0) {
        inByte = Serial.read();
        if(inByte == '8')
        {
            goForward();
        }
        if(inByte == '2'){
            goBackward();
        }
        if(inByte == '4')
            goLeft();
        if(inByte == '6')
            goRight();
        if(inByte == '5')
            stop();
    }
    Serial.println(inByte);
}

void irobot::goForward()
{
   // Drive op code
    softSerial.write(137);
    // Velocity (-500 ñ 500 mm/s)
    softSerial.write((byte)0);
    softSerial.write((byte)200);
    //Radius (-2000 - 2000 mm)
    softSerial.write((byte)128);
    softSerial.write((byte)0);
}

void irobot::goBackward()
{
    // Drive op code
    softSerial.write(137);
    // Velocity (-500 ñ 500 mm/s)
    softSerial.write(255);
    softSerial.write(56);
    //Radius (-2000 - 2000 mm)
    softSerial.write((byte)128);
    softSerial.write((byte)0);
}

void irobot::goLeft()
{
    // Drive op code
    softSerial.write(137);
    // Velocity (-500 ñ 500 mm/s)
    softSerial.write((byte)0);
    softSerial.write((byte)200);
    //Radius (-2000 - 2000 mm)
    softSerial.write((byte)0);
    softSerial.write((byte)1);
}

void irobot::goRight()
{
    // Drive op code
    softSerial.write(137);
    // Velocity (-500 ñ 500 mm/s)
    softSerial.write((byte)0);
    softSerial.write((byte)200);
    //Radius (-2000 - 2000 mm)
    softSerial.write((byte)255);
    softSerial.write((byte)255);
}

void irobot::stop()
{
    softSerial.write(137);
    softSerial.write((byte)0);
    softSerial.write((byte)0);
    //Radius (-2000 - 2000 mm)
    softSerial.write((byte)0);
    softSerial.write((byte)0);
}
</code>```

答:
All you need is a ultrasonic sensor to detect objects. You can buy one at aliexpress. Then you must download a library named NewPing Arduino Library for Arduino which is compatible with a wide range of ultrasonic sensors.

You can download it from Arduino New Ping at Bitbucket.

It contains functions to take distance between the sensor and the obstacle. Then you can set a condition using if statement that of the distance between the obstacle and robot is less than, say, 5 CM, then stop all motors. For more information consult instuctables website.


问:
Motion input in Simmechanics Matlab.
I have this figure in which motion is given to revolute joint in Simmechanics.

In this, constant 2 is used and then integration is used. I want to  know what will be effect of 2 here and how integration is happening. What is the actual value of input here. Is it 2 degree or 2 degree/sec or something else. Is 2 is the upper limit of motion or lower limit? What will happen if replace 2 with 5.

Thanks.

答:
2 represents a constant acceleration of the revolute joint. It's unit of measure cannot be determined from this screenshot, since it is a parameter of the joint actuator (to my knowledge it can be set to deg/sec^2 or rad/sec^2).

It effect is a constant acceleration to the joint. Let's assume that in the joint actuator block the unit of measure is set to deg/s^2. It this case your mechanism will rotate with an increasing velocity. After 1 second of simulation time the velocity will be 2 deg/s. After 2 seconds of simulation time its velocity will be 4 deg/s. and so on. There is no limit to what the velocity will reach, the longer the simulation time the faster your mechanism will spin.

Title: Motion input in Simmechanics Matlab.


问:
Inverse dynamics for robotic arm using Matlab Simmechanics
I am learning Simmechanics Matlab to do inverse dynamics for 4 DOF robotic arm. I read many examples to input motion to revolute joints like through PID, slider gain, sine waves, signal Builder etc. But these are not fulfilling my purpose as I have to rotate angles within limits and automatically. For example when I used sine wave signal, it continuously rotate until simulation time is not over. So, basically what I have is angles to rotate (through inverse kinematics) and now I want to find out torque required to reach that pose. How I can do this? How to create signal which fit in this scenario.

Thanks.

答:
For each revolute joint in the robot you can add a joint sensor. The sensors can be configured to output all joint forces and torques. That way you can compute reaction and motor forces and torques for a specified motion or pose of the robot. </p>

<p>Please make sure that you carefully select the reference frames for the measurements. </p>

Body:
For each revolute joint in the robot you can add a joint sensor. The sensors can be configured to output all joint forces and torques. That way you can compute reaction and motor forces and torques for a specified motion or pose of the robot.

Please make sure that you carefully select the reference frames for the measurements.


问:
information filter instead of kalman filter approach
I read many sources about kalman filter, yet no about the other approach to filtering, where canonical parametrization instead of moments parametrization is used.

What is the difference?

Other questions:

Using IF I can forget KF,but have to remember that prediction is more complicated link

How can I imagine uncertainty matrix turning into an ellipse? (generally I see, area is uncertainty, but I mean boundaries)

Simple addition of information in IF was possible only under assumption that each sensor read a different object? (hence no association problem, which I posted here

答:
They are exactly the same. Information matricies (aka precision matricies) are the inverse of covariance matricies. Follow this. The covariance update $$P_{+} = (I-KH)P$$ can be expanded by the definition of $K$ to be

$$ P_{+} = P - KHP $$
$$ P_{+} = P - PH^T (HPH^T+R)^{-1} HP $$

Now apply the matrix inversion lemma, and we have:

$$ P_{+} = P - PH^T (HPH^T+R)^{-1} HP $$
$$ P_{+} = (P^{-1} + H^TR^{-1}H)^{-1} $$

Which implies:
$$ P_{+}^{-1} = P^{-1} + H^TR^{-1}H $$

The term $P^{-1}$ is called the prior information, $H^TR^{-1}H$ is the sensor information (inverse of sensor variance), and this gives us $P^{-1}_+$, which is the posterior information.

I'm glossing over the actual state estimate, but it's straightforward. The best intro I've seen on this is not Thrun's book, but Ben Grocholsky's PhD thesis. (Just the intro material). It's called Information Theoretic Control of Multiple Sensor Platforms. Here's a link.

EDITS

To answer the questions posed.

It is not more complicated to predict, it is more computationally costly, since you must invert the $n \times n$ covariance matrix to get the true state output.

To view the ellipse from a covariance matrix, just note that the covariance matrix has a nice Singular Value Decomposition. The square root of the eigenvalues of the ellipse, or the square root of the singular values of the ellipse, will define the principal axes of the ellipse.

No, addition of information depends only on the assumption of independence of measurement noise. If you want to use two information filters to track two objects, that's fine. Or if you want to use an IF to track two objects, that's also fine. All you need is the correct association of measurements, so that you know which part of the state (object 1 or object 2) to update.


问:
Camera selection for long range stereo vision system (up to 100 meters)
I want to implement a real-time stereo vision system for long range (up to 100m) depth estimation. I know that there are some general considerations as described in this SOV post. I have seen some typical cameras such as zed stereo cameras which has limited range (max. 20m).

Maximum allowable baseline distance between cameras is 0.5m and about field of view, i think that a camera with 8mm (or 12 or 16) focal length can provide reasonable FOV. I need depth resolustions at 100m to be 1% or maybe lower.

Are ip cameras proper for such applications? If no, Can anyone please suggest proper cameras for application in long range stereo vision system?

I will be grateful for any information you can provide.

答:
I still think this is off-topic, but it seems I need more space than a comment to show (answer?) why that is so.

You are starting from some performance specifications and are looking to get to a set of features you need in your camera.

Here is a post from NI about stereo vision that gives a formula for depth resolution:

$$
\Delta z = \frac{z^2}{fb}\Delta d \\
$$

where $z$ is the depth of the object from the stereo system, $\Delta d$ is the depth resolution, $f$ is the focal length of the camera, $b$ is the baseline, and $d$ is the image disparity.

So, you want 1% depth resolution at 100 meters, or a depth resolution of 1 meter. A focal length of 8 millimeters, or 0.008 m, and a baseline of 0.5 m.

Rearranging the equation, it looks like you'll need a camera capable of registering a disparity of:

$$
\Delta d = \Delta z \frac{fb}{z^2} \\
\Delta d = 1 \frac{(0.008)(0.5)}{100^2} \\
\Delta d = 4 x 10^-7 m \\
\Delta d = 0.4 \mu m \\
$$

Assuming pixel accuracy (not sub-pixel accuracy), you'll want one pixel to be 0.4 $\mu$m or smaller, so the 0.4 $\mu$m disparity is registered as a one pixel shift between cameras.

Here's a list of sensor formats and sizes. I'm assuming these cameras all do "full HD", at a resolution of 1920x1080. Looking at the 2/3" format, the sensor width is 8.8 mm.

You need to register 0.4 $\mu$m, how does that compare to the 2/3" format? Well, at a width of 0.0088 m, with 1920 pixels across that width, the 2/3" format has a pixel width of $0.0088/1920 = 4.58\mu m$. So, off by a factor of 10. You need the pixel width to be about 11 times smaller.

So let's look at the 1/3" format - as in the iPhone 6. There the width is 4.8mm, so about half as wide, meaning you still need the pixels to be about 5-6 times smaller than the camera sensor in the iPhone.

This is also assuming you want to use every pixel in a full HD format - this will result in a high computation time. Most of the stereo vision projects I've seen have used cameras with lower resolutions or downsampled the image to a format like 640x480, but of course that means that the pixels are much (3x) larger.

You ask if "IP Cameras" are "proper," but IP cameras come in lots of styles.

Hopefully this will help you as a guide for your iterations. Plainly speaking, I don't think you'll ever find anything (that is reasonably affordable) that would do the depth resolution at the baseline you're talking about. I would imagine the baseline would be more on the range of 5-10 meters to get what you need. At 10 meters, fyi, the pixel size becomes 8 $\mu$m. At that point, most/all of the HD cameras should be able to do what you want, but again HD is computationally expensive because there are so many pixels to correlate.

This will be an iterative process. Work forward and backward and forward and backward until you get the design that meets your needs. You'll find you need to make tradeoffs along the way, and that's the core of engineering - finding the "optimal" balance of specifications. Cost, performance, size, cost, weight, interface, lead time, cost, cost, cost.


答:
I'll try to explain this in terms of software. Firstly, it is just next to impossible to have 100 metres (practically, 3 metres is like the best depth accuracy) and we need to be sure that the cameras are capturing images properly with all practical features (vibration-free and even under sunlight or rain)! (Else features will be lost and we end up with almost 'plain' features resulting in poor mapping) While it has to deal largely with resolution, color, focal length etc, it is definitely practically impossible with normal IP camera and some specialised stereo-vision cameras may have to be employed as mentioned in Chuck's answer (0.4 micrometer?).

So, I would suggest the use of Stereo-vision or Laser scan for reinforcement learning (links not hyper-referenced deliberately).

"High Speed Obstacle Avoidance using Monocular Vision and Reinforcement Learning" http://www.cs.cornell.edu/~asaxena/rccar/icml_obstacleavoidance.pdf

"Depth Estimation using Monocular and Stereo Cues"

https://pdfs.semanticscholar.org/4953/1103099c8d17ea34eb09433688e84de4f35f.pdf

These describe detecting features and associating depth-information with every feature and training it continuously for better and even better results. First paper uses LIDAR and that can be a bit costly for you. As 2D LIDAR is being used, the features of the image are sampled in columns and those features are mapped to image-kernels and the depth is thus estimated. In second case, stereo-vision is being used to train (note: this is high quality data as this is used for training) and as we have complete 3D information, features are being trained in a similar fashion as deep-nets and depth information is being obtained by comparing layers of information in image. These techniques can identify upto 50m or so effectively.

Also, I would suggest OP about asking a single question instead of asking duplicates like this.

Summary: It is practically next to impossible to get 100m depth with off-shelf cameras and no additional softwares.

Title: Camera selection for long range stereo vision system (up to 100 meters)

Body:
I want to implement a real-time stereo vision system for long range (up to 100m) depth estimation. I know that there are some general considerations as described in this SOV post. I have seen some typical cameras such as zed stereo cameras which has limited range (max. 20m).

Maximum allowable baseline distance between cameras is 0.5m and about field of view, i think that a camera with 8mm (or 12 or 16) focal length can provide reasonable FOV. I need depth resolustions at 100m to be 1% or maybe lower.

Are ip cameras proper for such applications? If no, Can anyone please suggest proper cameras for application in long range stereo vision system?

I will be grateful for any information you can provide.


答:
This is called "data association" in tracking literature. When you measure the position of an object, you need to know which object it was you measured. If you can estimate this probability, then you are free to choose the most likely association. This is a heavily researched topic, but boils down to Bayesian analysis.

Here's a simple way:

Assume we have two objects $o_1$ and $o_2$, an estimate of their position $x_1$ and $x_2$, and a measurement $z$. Suppose $z$ is just a position measurement. We'd like to update $o_1$ or $o_2$'s estimated position, but we have no idea with object we just measured. So, we find the most likely object, and update that.

Estimate $p(z|x_1)$ and $p(z|x_2)$. The Kalman filter gives you the tools to do this.

IF $p(z|x_1)>p(z|x_2)$ AND $p(z|x_1)>\gamma$, then update $x_1$ using $z$, the Kalman filter gives you the tools to do this as well.

ELSE IF $p(z|x_2)>p(z|x_1)$ AND $p(z|x_2)>\gamma$, then update $x_2$ using $z$, the Kalman filter gives you the tools to do this as well.

ELSE, no probability is greater than $\gamma$, so we drop the measurement. You can set $\gamma$ to zero if you don't care.

Two things: First, you also need to know how to initialize an object. That's why $\gamma$ is important. If the measurement doesn't seem to match any known estimates, you might have just detected a new object.

Second, I have a suspicion that your question will be heavily edited, so I'm not putting a lot of details in just now.

EDIT
To address your newer, revised question:

You have a different problem, but it is totally related. First, you want to fuse objects. This is as simple as estimating the probability that all measurements of object 1 and all measurements of object 2 are in fact of the same object.  The algorithm is simple, but nearly intractable computationally. This is the same as track merger.

To really do this with objects in 3D, you need a model of the object which allows you to estimate the probability that two partially overlapping (or possibly just nearby) objects are in fact the same object). But this is exactly the same as the acceptance gate approach.

note

There are only a few really useful ways to mere measurements, and they are well defined. I strongly suggest if you wish to continue with abstract tracking and estimation tasks that you read Bar-Shalom's Tracking and Estimation book. Please get it from a library if you can. I just don't know of a better reference.

Title: object level sensor fusion for multiobject tracking

Body:
I want to fuse objects coming from several sensors, with different (sometimes overlapping!) fields of view. Having object lists, how can I determine whether some objects observed by different sensors are in fact the same object? Only then I can truly write an algorithm to predict future state of such an object.

From literature I read those 4 steps:

Plot to track association (first update tracks estimates and then associate by "acceptance gate" or by statistical approach PDAF or JPDAF)

Track smoothing (lots of algorithms for generating new improved estimate, e.g.: EKF, UKF, PF)

Track initiation (create new tracks from unassociated plots)

Track maintenance (delete a track if was not associated for last M turns. also: predict those tracks that were associated, their new location based on previous heading and speed)

So basically I am questioning point 1, acceptance gate. For a single sensor I can imagine it can be just a comparison of xy position of object and sensor measurement, velocity with heading eventually. My case is however, I have already ready object lists from each sensor in every cycle, there are some algorithms how to merge informations about an object collected by different sensors (great source is e.g. here: http://www.mathworks.de/matlabcentral/fileexchange/37807-measurement-fusion-state-vector-fusion), but question is how to decide which objects should be fused, and which left as they were? Fields of view may overlap partly, not totally.


问:
Is there a way to determine which degrees of freedom are lost in a robot at a singularity position by looking at the jacobian?
For a 6DoF robot with all revolute joints the Jacobian is given by:
$$
\mathbf{J} =
\begin{bmatrix}
\hat{z_0} \times (\vec{o_6}-\vec{o_0}) & \ldots & \hat{z_5} \times (\vec{o_6}-\vec{o_5})\\
\hat{z_0} & \ldots & \hat{z_5}
\end{bmatrix}
$$
where $z_i$ is the unit z axis of joint $i+1$(using DH params), $o_i$ is the origin of the coordinate frame connected to joint $i+1$, and $o_6$ is the origin of the end effector.  The jacobian matrix is the relationship between the Cartesian velocity vector and the joint velocity vector:
$$
\dot{\mathbf{X}}=
\begin{bmatrix}
\dot{x}\\
\dot{y}\\
\dot{z}\\
\dot{r_x}\\
\dot{r_y}\\
\dot{r_z}
\end{bmatrix}
=
\mathbf{J}
\begin{bmatrix}
\dot{\theta_1}\\
\dot{\theta_2}\\
\dot{\theta_3}\\
\dot{\theta_4}\\
\dot{\theta_5}\\
\dot{\theta_6}\\
\end{bmatrix}
=
\mathbf{J}\dot{\mathbf{\Theta}}
$$

Here is a singularity position of a Staubli TX90XL 6DoF robot:

$$
\mathbf{J} =
\begin{bmatrix}
          -50     &    -425     &    -750      &      0     &    -100      &      0\\
       612.92     &       0     &       0      &      0     &       0      &      0\\
            0     & -562.92     &       0      &      0     &       0      &      0\\
            0     &       0     &       0      &      0     &       0      &      0\\
            0     &       1     &       1      &      0     &       1      &      0\\
            1     &       0     &       0      &     -1     &       0      &     -1
\end{bmatrix}
$$

You can easily see that the 4th row corresponding to $\dot{r_x}$ is all zeros, which is exactly the lost degree of freedom in this position.

However, other cases are not so straightforward.

$$
\mathbf{J} =
\begin{bmatrix}
          -50   &   -324.52  &    -649.52   &         0   &   -86.603   &         0\\
       987.92   &         0  &          0   &         0   &         0   &         0\\
            0   &   -937.92  &       -375   &         0   &       -50   &         0\\
            0   &         0  &          0   &       0.5   &         0   &       0.5\\
            0   &         1  &          1   &         0   &         1   &         0\\
            1   &         0  &          0   &    -0.866   &         0   &    -0.866
\end{bmatrix}
$$

Here you can clearly see that joint 4 and joint 6 are aligned because the 4th and 6th columns are the same.  But it's not clear which Cartesian degree of freedom is lost (it should be a rotation about the end effector's x axis in red).

Even less straightforward are singularities at workspace limits.

$$
\mathbf{J} =
\begin{bmatrix}
          -50     &     650   &       325  &          0    &        0     &       0\\
       1275.8     &       0   &         0  &         50    &        0     &       0\\
            0     & -1225.8   &   -662.92  &          0    &     -100     &       0\\
            0     &       0   &         0  &    0.86603    &        0     &       1\\
            0     &       1   &         1  &          0    &        1     &       0\\
            1     &       0   &         0  &        0.5    &        0     &       0
\end{bmatrix}
$$

In this case, the robot is able to rotate $\dot{-r_y}$ but not $\dot{+r_y}$.  There are no rows full of zeros, or equal columns, or any clear linearly dependent columns/rows.

Is there a way to determine which degrees of freedom are lost by looking at the jacobian?

答: Please correct me if I am wrong. Let's check if the Jacobian matrix loses it's rank (for example using Matlab or Octave). This should indicate the singularity. Then let's try to check the rank of sub-matrices of the Jacobian, to find which degree of freedom is lost.</p>